- if (ao_flight_state != ao_flight_startup) {
-#if USE_KALMAN
- printf("time %7.2f accel %d pres %d k_height %8.2f k_speed %8.5f k_accel %8.5f\n",
- (double) ao_adc_static.tick / 100,
- ao_adc_static.accel,
- ao_adc_static.pres,
- to_double(ao_k_height),
- to_double(ao_k_speed),
- to_double(ao_k_accel));
-#else
- printf("time %g accel %d pres %d\n",
- (double) ao_adc_static.tick / 100,
- ao_adc_static.accel,
- ao_adc_static.pres);
-#endif
+ if (ao_flight_state == ao_flight_startup)
+ return;
+ {
+ double height = ao_pres_to_altitude(ao_raw_pres) - ao_ground_height;
+ double accel = ((ao_flight_ground_accel - ao_adc_static.accel) * GRAVITY * 2.0) /
+ (ao_config.accel_minus_g - ao_config.accel_plus_g);
+
+ if (!tick_offset)
+ tick_offset = ao_adc_static.tick;
+ if (!drogue_height && ao_flight_state >= ao_flight_drogue)
+ drogue_height = ao_k_height;
+ if (!main_height && ao_flight_state >= ao_flight_main)
+ main_height = ao_k_height;
+ if ((prev_tick - ao_adc_static.tick) > 0)
+ tick_offset += 65536;
+ prev_tick = ao_adc_static.tick;
+ printf("%7.2f height %g accel %g state %s k_height %g k_speed %g k_accel %g drogue %g main %g error %d\n",
+ (double) (ao_adc_static.tick + tick_offset) / 100,
+ height,
+ accel,
+ ao_state_names[ao_flight_state],
+ ao_k_height / 65536.0,
+ ao_k_speed / 65536.0 / 16.0,
+ ao_k_accel / 65536.0 / 16.0,
+ drogue_height / 65536.0,
+ main_height / 65536.0,
+ ao_error_h_sq_avg);