#include <ao_log.h>
#endif
+#include <ao_flight.h>
+
#if HAS_MPU6000 || HAS_MPU9250
#include <ao_quaternion.h>
#endif
* resting
*/
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 ao_v_t ao_coast_avg_accel;
#endif
+#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
#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)
{
#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
+ )
{
+ 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
*/
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:
*
*/
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 */
+#if HAS_LOG
ao_log_start();
+#endif
#if HAS_TELEMETRY
/* Increase telemetry rate */
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:
(int16_t) (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_motor_number;
ao_wakeup(&ao_flight_state);
+ ++ao_motor_number;
}
break;
-#if HAS_ACCEL
+#if HAS_ACCEL && HAS_BARO
case ao_flight_fast:
/*
* This is essentially the same as coast,
#endif
case ao_flight_coast:
+#if HAS_BARO
/*
* By customer request - allow the user
* to lock out apogee detection for a specified
#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);
/* 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);
- /* and enter drogue state */
- ao_flight_state = ao_flight_drogue;
- ao_wakeup(&ao_flight_state);
+#define MAX_QUIET_ACCEL 2
+
+ if ((int16_t) (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
+ }
+
+ /* 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:
+#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;
#endif
break;
+#if HAS_BARO
case ao_flight_drogue:
/* drogue to main deploy:
*/
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
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;
*
* 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)
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;
}
break;
+#endif /* HAS_BARO */
#if HAS_FLIGHT_DEBUG
case ao_flight_test:
#if HAS_GYRO
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
printf (" max_height %d\n", ao_max_height);
printf (" avg_height %d\n", ao_avg_height);
printf (" error_h %d\n", ao_error_h);
+#if !HAS_ACCEL
printf (" error_avg %d\n", ao_error_h_sq_avg);
+#endif
}
static void
projects / fw / altos / blobdiff