X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fkernel%2Fao_flight.c;h=cee8b2b8b38d008c16460b5c293b8c4b326c005d;hb=7917ec1f105f39799acbea0f4c28d25db4f66eb4;hp=2142546c9b0737b186cca6f6dce65fa51c8e30ec;hpb=3058ecdeafdee959b1103965f3df4ffcd59683e4;p=fw%2Faltos diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c index 2142546c..cee8b2b8 100644 --- a/src/kernel/ao_flight.c +++ b/src/kernel/ao_flight.c @@ -50,8 +50,8 @@ /* Main flight thread. */ 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 */ +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 @@ -63,7 +63,7 @@ uint8_t ao_sensor_errors; * track min/max data over a long interval to detect * resting */ -static uint16_t ao_interval_end; +static AO_TICK_TYPE ao_interval_end; #ifdef HAS_BARO static ao_v_t ao_interval_min_height; static ao_v_t ao_interval_max_height; @@ -102,6 +102,10 @@ 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) { @@ -127,8 +131,8 @@ 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_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 @@ -200,6 +204,15 @@ ao_flight(void) 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: * @@ -216,8 +229,8 @@ 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 ) { @@ -259,7 +272,7 @@ ao_flight(void) * (15 seconds) has past. */ if ((ao_accel < AO_MSS_TO_ACCEL(-2.5)) || - (int16_t) (ao_sample_tick - ao_boost_tick) > BOOST_TICKS_MAX) + (AO_TICK_SIGNED) (ao_sample_tick - ao_boost_tick) > (AO_TICK_SIGNED) BOOST_TICKS_MAX) { #if HAS_ACCEL #if HAS_BARO @@ -306,8 +319,8 @@ ao_flight(void) * number of seconds. */ if (ao_config.apogee_lockout) { - if ((int16_t) (ao_sample_tick - ao_launch_tick) < - AO_SEC_TO_TICKS(ao_config.apogee_lockout)) + if ((AO_TICK_SIGNED) (ao_sample_tick - ao_launch_tick) < + (AO_TICK_SIGNED) AO_SEC_TO_TICKS(ao_config.apogee_lockout)) break; } @@ -350,7 +363,7 @@ ao_flight(void) #define MAX_QUIET_ACCEL 2 - 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_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)) @@ -430,7 +443,7 @@ ao_flight(void) 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; @@ -448,7 +461,7 @@ ao_flight(void) #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, @@ -479,14 +492,14 @@ 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); + printf (" raw pres %ld\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); + 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); @@ -495,8 +508,8 @@ ao_flight_dump(void) #endif #if HAS_BARO - printf (" alt %d\n", ao_sample_alt); - printf (" height %d\n", ao_sample_height); + printf (" alt %ld\n", ao_sample_alt); + printf (" height %ld\n", ao_sample_height); #endif #if HAS_ACCEL @@ -505,12 +518,12 @@ ao_flight_dump(void) 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 @@ -530,6 +543,7 @@ static void ao_orient_test_select(void) { ao_orient_test = !ao_orient_test; + printf("orient test %d\n", ao_orient_test); } const struct ao_cmds ao_flight_cmds[] = {