Hacks to allow accel-only flight code

EasyTimer won't have a baro sensor, so we need some way to track at least
the ascent part of a flight.

Signed-off-by: Keith Packard <keithp@keithp.com>

diff --git a/src/drivers/ao_mpu9250.c b/src/drivers/ao_mpu9250.c
index 1b31edf4611754fe1b8c8d8fe52562b161e8579e..0597a81c57fbc8677bc64d93002db7d0d0ca88b5 100644 (file)
--- a/src/drivers/ao_mpu9250.c
+++ b/src/drivers/ao_mpu9250.c
@@ -34,7 +34,11 @@ static uint8_t       ao_mpu9250_configured;
 
 #if AO_MPU9250_SPI
 
-#define ao_mpu9250_spi_get()   ao_spi_get(AO_MPU9250_SPI_BUS, AO_SPI_SPEED_1MHz)
+#ifndef AO_MPU9250_SPI_SPEED
+#define AO_MPU9250_SPI_SPEED   AO_SPI_SPEED_1MHz
+#endif
+
+#define ao_mpu9250_spi_get()   ao_spi_get(AO_MPU9250_SPI_BUS, AO_MPU9250_SPI_SPEED)
 #define ao_mpu9250_spi_put()   ao_spi_put(AO_MPU9250_SPI_BUS)
 
 #define ao_mpu9250_spi_start()         ao_spi_set_cs(AO_MPU9250_SPI_CS_PORT,   \
@@ -559,7 +563,7 @@ ao_mpu9250_init(void)
         */
 
        ao_cur_task = &ao_mpu9250_task;
-       ao_spi_get(AO_MPU9250_SPI_BUS, AO_SPI_SPEED_1MHz);
+       ao_mpu9250_spi_get();
        ao_cur_task = NULL;
 #endif
        ao_cmd_register(&ao_mpu9250_cmds[0]);

diff --git a/src/kernel/ao_cmd.c b/src/kernel/ao_cmd.c
index d1c049ac78aaf5b03905dc13542905f74cb01d71..9bc19038d6c6c82e65c4251d8ddf44ca8499cc06 100644 (file)
--- a/src/kernel/ao_cmd.c
+++ b/src/kernel/ao_cmd.c
@@ -269,7 +269,7 @@ version(void)
        printf("manufacturer     %s\n"
               "product          %s\n"
               "serial-number    %u\n"
-#if HAS_FLIGHT || HAS_TRACKER
+#if HAS_LOG && (HAS_FLIGHT || HAS_TRACKER)
               "current-flight   %u\n"
 #endif
 #if HAS_LOG
@@ -287,7 +287,7 @@ version(void)
               , ao_manufacturer
               , ao_product
               , ao_serial_number
-#if HAS_FLIGHT || HAS_TRACKER
+#if HAS_LOG && (HAS_FLIGHT || HAS_TRACKER)
               , ao_flight_number
 #endif
 #if HAS_LOG

diff --git a/src/kernel/ao_data.h b/src/kernel/ao_data.h
index 988ac4897ecafbb8798ce1e1b5ceb2bf09368144..56fada68bd381635c3f1a8bf309b3ce487f6142e 100644 (file)
--- a/src/kernel/ao_data.h
+++ b/src/kernel/ao_data.h
@@ -114,6 +114,9 @@ struct ao_data {
 #endif
 #if HAS_MPU9250
        struct ao_mpu9250_sample        mpu9250;
+#if !HAS_MMA655X
+       int16_t                         z_accel;
+#endif
 #endif
 #if HAS_HMC5883
        struct ao_hmc5883_sample        hmc5883;
@@ -188,20 +191,6 @@ typedef AO_ALT_TYPE        alt_t;
 
 #endif
 
-#if !HAS_BARO && HAS_ADC
-
-#define HAS_BARO       1
-
-typedef int16_t pres_t;
-typedef int16_t alt_t;
-
-#define ao_data_pres(packet)   ((packet)->adc.pres)
-#define ao_data_temp(packet)   ((packet)->adc.temp)
-#define pres_to_altitude(p)    ao_pres_to_altitude(p)
-#define ao_data_pres_cook(p)
-
-#endif
-
 /*
  * Need a few macros to pull data from the sensors:
  *
@@ -408,6 +397,20 @@ static inline float ao_convert_accel(int16_t sensor)
 
 #endif
 
+#if !HAS_ACCEL && HAS_MPU9250
+
+#define HAS_ACCEL      1
+
+typedef int16_t accel_t;
+
+/* MPU9250 is hooked up so that positive y is positive acceleration */
+#define ao_data_accel(packet)                  ((packet)->z_accel)
+#define ao_data_accel_cook(packet)             (-(packet)->mpu9250.accel_y)
+#define ao_data_set_accel(packet, accel)       ((packet)->z_accel = (accel))
+#define ao_data_accel_invert(a)                        (-(a))
+
+#endif
+
 #if !HAS_GYRO && HAS_MPU9250
 
 #define HAS_GYRO       1

diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c
index 5a5d5b72b0adfb887f238d6512c5b91b68b8a016..3e3ee77ac97fbc224430b22836ef8d42dece8290 100644 (file)
--- a/src/kernel/ao_flight.c
+++ b/src/kernel/ao_flight.c
@@ -108,10 +108,12 @@ 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 < ao_config.accel_plus_g - ACCEL_NOSE_UP
+#if HAS_BARO
+                           || ao_ground_accel > ao_config.accel_minus_g + ACCEL_NOSE_UP
+                           || ao_ground_height < -1000 || ao_ground_height > 7000
+#endif
+                               )
                        {
                                /* Detected an accel value outside -1.5g to 1.5g
                                 * (or uncalibrated values), so we go into invalid mode
@@ -203,7 +205,9 @@ ao_flight(void)
                                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 */

diff --git a/src/kernel/ao_kalman.c b/src/kernel/ao_kalman.c
index 4f4ffe8f1b6a813b532cd4fe751ed6d684f34b6b..4e7550c56dcd95d99d5730a6757c5fd8dc52dcd1 100644 (file)
--- a/src/kernel/ao_kalman.c
+++ b/src/kernel/ao_kalman.c
@@ -41,8 +41,10 @@ ao_v_t                       ao_height;
 ao_v_t                 ao_speed;
 ao_v_t                 ao_accel;
 ao_v_t                 ao_max_height;
+#if HAS_BARO
 static ao_k_t          ao_avg_height_scaled;
 ao_v_t                 ao_avg_height;
+#endif
 
 ao_v_t                 ao_error_h;
 #if !HAS_ACCEL || AO_FLIGHT_TEST
@@ -86,6 +88,7 @@ ao_kalman_predict(void)
        ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_100;
 }
 
+#if HAS_BARO
 static void
 ao_kalman_err_height(void)
 {
@@ -140,7 +143,9 @@ ao_kalman_err_height(void)
 #endif
        }
 }
+#endif
 
+#if HAS_BARO
 static void
 ao_kalman_correct_baro(void)
 {
@@ -163,6 +168,7 @@ ao_kalman_correct_baro(void)
        ao_k_speed  += (ao_k_t) AO_BARO_K1_100 * ao_error_h;
        ao_k_accel  += (ao_k_t) AO_BARO_K2_100 * ao_error_h;
 }
+#endif
 
 #if HAS_ACCEL
 
@@ -177,7 +183,7 @@ ao_kalman_err_accel(void)
        ao_error_a = (accel - ao_k_accel) >> 16;
 }
 
-#ifndef FORCE_ACCEL
+#if !defined(FORCE_ACCEL) && HAS_BARO
 static void
 ao_kalman_correct_both(void)
 {
@@ -255,12 +261,14 @@ ao_kalman_correct_accel(void)
 {
        ao_kalman_err_accel();
 
+#ifdef AO_FLIGHT_TEST
        if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 5) {
                ao_k_height +=(ao_k_t) AO_ACCEL_K0_10 * ao_error_a;
                ao_k_speed  += (ao_k_t) AO_ACCEL_K1_10 * ao_error_a;
                ao_k_accel  += (ao_k_t) AO_ACCEL_K2_10 * ao_error_a;
                return;
        }
+#endif
        ao_k_height += (ao_k_t) AO_ACCEL_K0_100 * ao_error_a;
        ao_k_speed  += (ao_k_t) AO_ACCEL_K1_100 * ao_error_a;
        ao_k_accel  += (ao_k_t) AO_ACCEL_K2_100 * ao_error_a;
@@ -273,6 +281,7 @@ void
 ao_kalman(void)
 {
        ao_kalman_predict();
+#if HAS_BARO
 #if HAS_ACCEL
        if (ao_flight_state <= ao_flight_coast) {
 #ifdef FORCE_ACCEL
@@ -283,11 +292,15 @@ ao_kalman(void)
        } else
 #endif
                ao_kalman_correct_baro();
+#else
+       ao_kalman_correct_accel();
+#endif
        ao_height = from_fix(ao_k_height);
        ao_speed = from_fix(ao_k_speed);
        ao_accel = from_fix(ao_k_accel);
        if (ao_height > ao_max_height)
                ao_max_height = ao_height;
+#if HAS_BARO
        ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_sample_height;
 #ifdef AO_FLIGHT_TEST
        if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 50)
@@ -297,4 +310,5 @@ ao_kalman(void)
        else 
 #endif
                ao_avg_height = (ao_avg_height_scaled + 63) >> 7;
+#endif
 }

diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c
index 9cba36c1004b37ec31717e7c4431e08ede4bf615..b3e12b19df7579ff50734defc8153fa9247db8d3 100644 (file)
--- a/src/kernel/ao_sample.c
+++ b/src/kernel/ao_sample.c
@@ -36,9 +36,11 @@
 #endif
 
 uint16_t       ao_sample_tick;         /* time of last data */
+#if HAS_BARO
 pres_t         ao_sample_pres;
 alt_t          ao_sample_alt;
 alt_t          ao_sample_height;
+#endif
 #if HAS_ACCEL
 accel_t                ao_sample_accel;
 #endif
@@ -60,8 +62,10 @@ uint8_t              ao_sample_data;
  * Sensor calibration values
  */
 
+#if HAS_BARO
 pres_t         ao_ground_pres;         /* startup pressure */
 alt_t          ao_ground_height;       /* MSL of ao_ground_pres */
+#endif
 
 #if HAS_ACCEL
 accel_t                ao_ground_accel;        /* startup acceleration */
@@ -81,7 +85,9 @@ int32_t               ao_ground_roll;
 static uint8_t ao_preflight;           /* in preflight mode */
 
 static uint16_t        nsamples;
+#if HAS_BARO
 int32_t ao_sample_pres_sum;
+#endif
 #if HAS_ACCEL
 int32_t ao_sample_accel_sum;
 #endif
@@ -105,7 +111,9 @@ ao_sample_preflight_add(void)
 #if HAS_ACCEL
        ao_sample_accel_sum += ao_sample_accel;
 #endif
+#if HAS_BARO
        ao_sample_pres_sum += ao_sample_pres;
+#endif
 #if HAS_GYRO
        ao_sample_accel_along_sum += ao_sample_accel_along;
        ao_sample_accel_across_sum += ao_sample_accel_across;
@@ -171,9 +179,11 @@ ao_sample_preflight_set(void)
        ao_ground_accel = ao_sample_accel_sum >> 9;
        ao_sample_accel_sum = 0;
 #endif
+#if HAS_BARO
        ao_ground_pres = ao_sample_pres_sum >> 9;
        ao_ground_height = pres_to_altitude(ao_ground_pres);
        ao_sample_pres_sum = 0;
+#endif
 #if HAS_GYRO
        ao_ground_accel_along = ao_sample_accel_along_sum >> 9;
        ao_ground_accel_across = ao_sample_accel_across_sum >> 9;
@@ -375,8 +385,10 @@ ao_sample_init(void)
 {
        ao_config_get();
        nsamples = 0;
+#if HAS_BARO
        ao_sample_pres_sum = 0;
        ao_sample_pres = 0;
+#endif
 #if HAS_ACCEL
        ao_sample_accel_sum = 0;
        ao_sample_accel = 0;