From: Keith Packard Date: Tue, 13 Aug 2019 00:00:47 +0000 (-0700) Subject: Hacks to allow accel-only flight code X-Git-Url: https://git.gag.com/?a=commitdiff_plain;h=df99adeb60451d40287e91335a8b3f454baa2aaa;p=fw%2Faltos 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 --- diff --git a/src/drivers/ao_mpu9250.c b/src/drivers/ao_mpu9250.c index 1b31edf4..0597a81c 100644 --- 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 d1c049ac..9bc19038 100644 --- 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 988ac489..56fada68 100644 --- 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 5a5d5b72..3e3ee77a 100644 --- 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 4f4ffe8f..4e7550c5 100644 --- 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 9cba36c1..b3e12b19 100644 --- 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;