From b1d37be4c024e9690107c693d9819229025966fa Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 17 Dec 2012 17:03:41 -0800 Subject: altos: Average MPU6000 values on ground for later use Having long-term ground averages recorded to the eeprom file will make post-flight analysis of the data better. Signed-off-by: Keith Packard --- src/core/ao_sample.c | 95 +++++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 94 insertions(+), 1 deletion(-) (limited to 'src/core/ao_sample.c') diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c index 985c0940..7a1eff8e 100644 --- a/src/core/ao_sample.c +++ b/src/core/ao_sample.c @@ -37,6 +37,16 @@ __pdata alt_t ao_sample_height; #if HAS_ACCEL __pdata accel_t ao_sample_accel; #endif +#if HAS_GYRO +__pdata accel_t ao_sample_accel_along; +__pdata accel_t ao_sample_accel_across; +__pdata accel_t ao_sample_accel_through; +__pdata gyro_t ao_sample_roll; +__pdata gyro_t ao_sample_pitch; +__pdata gyro_t ao_sample_yaw; +__pdata angle_t ao_sample_angle; +__pdata angle_t ao_sample_roll_angle; +#endif __data uint8_t ao_sample_data; @@ -53,6 +63,15 @@ __pdata accel_t ao_accel_2g; /* factory accel calibration */ __pdata int32_t ao_accel_scale; /* sensor to m/s² conversion */ #endif +#if HAS_GYRO +__pdata accel_t ao_ground_accel_along; +__pdata accel_t ao_ground_accel_across; +__pdata accel_t ao_ground_accel_through; +__pdata gyro_t ao_ground_pitch; +__pdata gyro_t ao_ground_yaw; +__pdata gyro_t ao_ground_roll; +#endif + static __pdata uint8_t ao_preflight; /* in preflight mode */ static __pdata uint16_t nsamples; @@ -60,6 +79,14 @@ __pdata int32_t ao_sample_pres_sum; #if HAS_ACCEL __pdata int32_t ao_sample_accel_sum; #endif +#if HAS_GYRO +__pdata int32_t ao_sample_accel_along_sum; +__pdata int32_t ao_sample_accel_across_sum; +__pdata int32_t ao_sample_accel_through_sum; +__pdata int32_t ao_sample_pitch_sum; +__pdata int32_t ao_sample_yaw_sum; +__pdata int32_t ao_sample_roll_sum; +#endif static void ao_sample_preflight_add(void) @@ -68,6 +95,14 @@ ao_sample_preflight_add(void) ao_sample_accel_sum += ao_sample_accel; #endif ao_sample_pres_sum += ao_sample_pres; +#if HAS_GYRO + ao_sample_accel_along_sum += ao_sample_accel_along; + ao_sample_accel_across_sum += ao_sample_accel_across; + ao_sample_accel_through_sum += ao_sample_accel_through; + ao_sample_pitch_sum += ao_sample_pitch; + ao_sample_yaw_sum += ao_sample_yaw; + ao_sample_roll_sum += ao_sample_roll; +#endif ++nsamples; } @@ -80,8 +115,23 @@ ao_sample_preflight_set(void) #endif ao_ground_pres = ao_sample_pres_sum >> 9; ao_ground_height = pres_to_altitude(ao_ground_pres); - nsamples = 0; ao_sample_pres_sum = 0; +#if HAS_GYRO + ao_ground_accel_along = ao_sample_accel_along_sum >> 9; + ao_ground_accel_across = ao_sample_accel_across_sum >> 9; + ao_ground_accel_through = ao_sample_accel_through_sum >> 9; + ao_ground_pitch = ao_sample_pitch_sum >> 9; + ao_ground_yaw = ao_sample_yaw_sum >> 9; + ao_ground_roll = ao_sample_roll_sum >> 9; + ao_sample_accel_along_sum = 0; + ao_sample_accel_across_sum = 0; + ao_sample_accel_through_sum = 0; + ao_sample_pitch_sum = 0; + ao_sample_yaw_sum = 0; + ao_sample_roll_sum = 0; + ao_sample_angle = 0; +#endif + nsamples = 0; } static void @@ -122,6 +172,24 @@ ao_sample_preflight_update(void) ao_sample_preflight_set(); } +#if HAS_GYRO + +static int32_t p_filt; +static int32_t y_filt; + +static gyro_t inline ao_gyro(void) { + gyro_t p = ao_sample_pitch - ao_ground_pitch; + gyro_t y = ao_sample_yaw - ao_ground_yaw; + + p_filt = p_filt - (p_filt >> 6) + p; + y_filt = y_filt - (y_filt >> 6) + y; + + p = p_filt >> 6; + y = y_filt >> 6; + return ao_sqrt(p*p + y*y); +} +#endif + uint8_t ao_sample(void) { @@ -147,6 +215,12 @@ ao_sample(void) ao_sample_accel = ao_data_accel_invert(ao_sample_accel); ao_data_set_accel(ao_data, ao_sample_accel); #endif +#if HAS_GYRO + ao_sample_accel_along = ao_data_along(ao_data); + ao_sample_pitch = ao_data_pitch(ao_data); + ao_sample_yaw = ao_data_yaw(ao_data); + ao_sample_roll = ao_data_roll(ao_data); +#endif if (ao_preflight) ao_sample_preflight(); @@ -154,6 +228,10 @@ ao_sample(void) if (ao_flight_state < ao_flight_boost) ao_sample_preflight_update(); ao_kalman(); +#if HAS_GYRO + ao_sample_angle += ao_gyro(); + ao_sample_roll_angle += (ao_sample_roll - ao_ground_roll); +#endif } ao_sample_data = ao_data_ring_next(ao_sample_data); } @@ -170,6 +248,21 @@ ao_sample_init(void) #if HAS_ACCEL ao_sample_accel_sum = 0; ao_sample_accel = 0; +#endif +#if HAS_GYRO + ao_sample_accel_along_sum = 0; + ao_sample_accel_across_sum = 0; + ao_sample_accel_through_sum = 0; + ao_sample_accel_along = 0; + ao_sample_accel_across = 0; + ao_sample_accel_through = 0; + ao_sample_pitch_sum = 0; + ao_sample_yaw_sum = 0; + ao_sample_roll_sum = 0; + ao_sample_pitch = 0; + ao_sample_yaw = 0; + ao_sample_roll = 0; + ao_sample_angle = 0; #endif ao_sample_data = ao_data_head; ao_preflight = TRUE; -- cgit v1.2.3 From 57487e78b90465a21c87cf30deb0aeaba0887332 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 18 Dec 2012 23:15:20 -0800 Subject: altos: Actually record ground averages for 6dof sensor This gets the long-term averages for the 6dof sensors recorded into the first flight log record. Signed-off-by: Keith Packard --- src/core/ao_log.h | 6 +++--- src/core/ao_log_mega.c | 8 ++++++++ src/core/ao_sample.c | 8 +++++--- src/core/ao_sample.h | 8 ++++++++ 4 files changed, 24 insertions(+), 6 deletions(-) (limited to 'src/core/ao_sample.c') diff --git a/src/core/ao_log.h b/src/core/ao_log.h index 93b01778..036d6f2d 100644 --- a/src/core/ao_log.h +++ b/src/core/ao_log.h @@ -205,9 +205,9 @@ struct ao_log_mega { int16_t ground_accel_along; /* 16 */ int16_t ground_accel_across; /* 12 */ int16_t ground_accel_through; /* 14 */ - int16_t ground_gyro_roll; /* 18 */ - int16_t ground_gyro_pitch; /* 20 */ - int16_t ground_gyro_yaw; /* 22 */ + int16_t ground_roll; /* 18 */ + int16_t ground_pitch; /* 20 */ + int16_t ground_yaw; /* 22 */ } flight; /* 24 */ /* AO_LOG_STATE */ struct { diff --git a/src/core/ao_log_mega.c b/src/core/ao_log_mega.c index ac1590db..e03687ad 100644 --- a/src/core/ao_log_mega.c +++ b/src/core/ao_log_mega.c @@ -94,6 +94,14 @@ ao_log(void) log.tick = ao_sample_tick; #if HAS_ACCEL log.u.flight.ground_accel = ao_ground_accel; +#endif +#if HAS_GYRO + log.u.flight.ground_accel_along = ao_ground_accel_along; + log.u.flight.ground_accel_across = ao_ground_accel_across; + log.u.flight.ground_accel_through = ao_ground_accel_through; + log.u.flight.ground_pitch = ao_ground_pitch; + log.u.flight.ground_yaw = ao_ground_yaw; + log.u.flight.ground_roll = ao_ground_roll; #endif log.u.flight.ground_pres = ao_ground_pres; log.u.flight.flight = ao_flight_number; diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c index 7a1eff8e..dec44f9f 100644 --- a/src/core/ao_sample.c +++ b/src/core/ao_sample.c @@ -172,8 +172,8 @@ ao_sample_preflight_update(void) ao_sample_preflight_set(); } +#if 0 #if HAS_GYRO - static int32_t p_filt; static int32_t y_filt; @@ -189,6 +189,7 @@ static gyro_t inline ao_gyro(void) { return ao_sqrt(p*p + y*y); } #endif +#endif uint8_t ao_sample(void) @@ -217,6 +218,8 @@ ao_sample(void) #endif #if HAS_GYRO ao_sample_accel_along = ao_data_along(ao_data); + ao_sample_accel_across = ao_data_across(ao_data); + ao_sample_accel_through = ao_data_through(ao_data); ao_sample_pitch = ao_data_pitch(ao_data); ao_sample_yaw = ao_data_yaw(ao_data); ao_sample_roll = ao_data_roll(ao_data); @@ -229,8 +232,7 @@ ao_sample(void) ao_sample_preflight_update(); ao_kalman(); #if HAS_GYRO - ao_sample_angle += ao_gyro(); - ao_sample_roll_angle += (ao_sample_roll - ao_ground_roll); + /* do quaternion stuff here... */ #endif } ao_sample_data = ao_data_ring_next(ao_sample_data); diff --git a/src/core/ao_sample.h b/src/core/ao_sample.h index 9336bdf9..a2dac979 100644 --- a/src/core/ao_sample.h +++ b/src/core/ao_sample.h @@ -111,6 +111,14 @@ extern __pdata accel_t ao_ground_accel; /* startup acceleration */ extern __pdata accel_t ao_accel_2g; /* factory accel calibration */ extern __pdata int32_t ao_accel_scale; /* sensor to m/s² conversion */ #endif +#if HAS_GYRO +extern __pdata accel_t ao_ground_accel_along; +extern __pdata accel_t ao_ground_accel_across; +extern __pdata accel_t ao_ground_accel_through; +extern __pdata gyro_t ao_ground_pitch; +extern __pdata gyro_t ao_ground_yaw; +extern __pdata gyro_t ao_ground_roll; +#endif void ao_sample_init(void); -- cgit v1.2.3