X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fkernel%2Fao_flight.c;h=4b6bfd8f7109be78233e25f13f2aa9317410bbbb;hb=0136cef90ef9ac2a7325df588d12db3883f509d0;hp=afee1de710935b00c6a75e3b3f2498beb23be6a8;hpb=2f32a19aedb7d0e33899038e3fb04fe8a773291a;p=fw%2Faltos diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c index afee1de7..4b6bfd8f 100644 --- a/src/kernel/ao_flight.c +++ b/src/kernel/ao_flight.c @@ -102,6 +102,13 @@ uint8_t ao_flight_force_idle; #define abs(a) ((a) < 0 ? -(a) : (a)) +static bool accel_plus_g_failed; +static bool accel_minus_g_failed; +static bool accel_plus_failed; +static bool accel_minus_failed; + +static char *btos(bool x) { return x? "true" : "false"; } + void ao_flight(void) { @@ -127,14 +134,22 @@ 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 #endif ) { + if (ao_config.accel_plus_g == 0) + accel_plus_g_failed = true; + if (ao_config.accel_minus_g == 0) + accel_minus_g_failed = true; + if (ao_ground_accel < (accel_t) ao_config.accel_plus_g - ACCEL_NOSE_UP) + accel_plus_failed = true; + if (ao_ground_accel > (accel_t) ao_config.accel_minus_g + ACCEL_NOSE_UP) + accel_minus_failed = true; /* Detected an accel value outside -1.5g to 1.5g * (or uncalibrated values), so we go into invalid mode */ @@ -200,6 +215,25 @@ ao_flight(void) ao_wakeup(&ao_flight_state); break; + + case ao_flight_invalid: + printf("+g? %s -g? %s +? %s -? %s +g %d -g %d ga %d +g-NU %d -g+NU %d\n", + btos(accel_plus_g_failed), + btos(accel_minus_g_failed), + btos(accel_plus_failed), + btos(accel_minus_failed), + ao_config.accel_plus_g, + ao_config.accel_minus_g, + ao_ground_accel, + ao_config.accel_plus_g - ACCEL_NOSE_UP, + ao_config.accel_minus_g + ACCEL_NOSE_UP); + break; +#if 0 + 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,12 +250,17 @@ 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) +#if HAS_BARO + && ao_speed > AO_MS_TO_SPEED(5) +#endif +) #endif ) { ao_flight_state = ao_flight_boost; + ao_wakeup(&ao_flight_state); + ao_launch_tick = ao_boost_tick = ao_sample_tick; /* start logging data */ @@ -242,8 +281,6 @@ ao_flight(void) ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING; ao_wakeup(&ao_gps_new); #endif - - ao_wakeup(&ao_flight_state); } break; case ao_flight_boost: @@ -278,8 +315,8 @@ ao_flight(void) #else ao_flight_state = ao_flight_coast; #endif - ++ao_motor_number; ao_wakeup(&ao_flight_state); + ++ao_motor_number; } break; #if HAS_ACCEL && HAS_BARO @@ -327,11 +364,9 @@ ao_flight(void) #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); @@ -339,10 +374,6 @@ ao_flight(void) /* Turn the RDF beacon back on */ ao_rdf_set(1); #endif - - /* and enter drogue state */ - ao_flight_state = ao_flight_drogue; - ao_wakeup(&ao_flight_state); } else #else /* not HAS_BARO */ @@ -362,12 +393,11 @@ ao_flight(void) 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 - - ao_wakeup(&ao_flight_state); } /* Reset interval values */ @@ -410,9 +440,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 @@ -423,9 +452,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(&ao_flight_state); } break; @@ -445,13 +471,11 @@ ao_flight(void) 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(&ao_flight_state); } ao_interval_min_height = ao_interval_max_height = ao_avg_height; ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; @@ -491,20 +515,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