From 39c5738acdfdf0c87b64de6135fe107971cfa12b Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 8 Oct 2012 23:04:16 -0700 Subject: altos: Go back to recording sensor data in globals Instead of trying to get things into the ring from a variety of functions, go back to the simpler method of storing them in globals and having the ADC code just pluck out the most recent values. Signed-off-by: Keith Packard --- src/core/ao_data.h | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) (limited to 'src/core/ao_data.h') diff --git a/src/core/ao_data.h b/src/core/ao_data.h index 2b9ef5ac..30208dfb 100644 --- a/src/core/ao_data.h +++ b/src/core/ao_data.h @@ -85,14 +85,7 @@ extern volatile __data uint8_t ao_data_count; /* * Mark a section of data as ready, check for data complete */ -#define AO_DATA_PRESENT(bit) do { \ - if ((ao_data_present |= (bit)) == AO_DATA_ALL) { \ - ao_data_ring[ao_data_head].tick = ao_tick_count; \ - ao_data_head = ao_data_ring_next(ao_data_head); \ - ao_data_present = 0; \ - ao_wakeup((void *) &ao_data_head); \ - } \ - } while (0); +#define AO_DATA_PRESENT(bit) (ao_data_present |= (bit)) /* * Wait until it is time to write a sensor sample; this is -- cgit v1.2.3 From 175380a436efa35bbfae2ee5e29e12e9ef86fbde Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 12 Oct 2012 13:57:49 -0700 Subject: altos: Use alt_t for all Pascal-based altitude data This allows alt_t to be overridden for systems using the MS5607/MS5611 sensors Signed-off-by: Keith Packard --- src/core/ao.h | 6 ++++-- src/core/ao_convert_pa.c | 6 +++--- src/core/ao_convert_pa_test.c | 1 + src/core/ao_data.h | 11 ++++++++++- 4 files changed, 18 insertions(+), 6 deletions(-) (limited to 'src/core/ao_data.h') diff --git a/src/core/ao.h b/src/core/ao.h index 200d4bc4..e559e876 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -272,11 +272,13 @@ ao_temp_to_dC(int16_t temp) __reentrant; * Convert between pressure in Pa and altitude in meters */ -int32_t +#include + +alt_t ao_pa_to_altitude(int32_t pa); int32_t -ao_altitude_to_pa(int32_t alt); +ao_altitude_to_pa(alt_t alt); #if HAS_DBG #include diff --git a/src/core/ao_convert_pa.c b/src/core/ao_convert_pa.c index 0c93caea..1413681d 100644 --- a/src/core/ao_convert_pa.c +++ b/src/core/ao_convert_pa.c @@ -26,7 +26,7 @@ static const int32_t altitude_table[] = { #define ALT_SCALE (1 << ALT_SHIFT) #define ALT_MASK (ALT_SCALE - 1) -int32_t +alt_t ao_pa_to_altitude(int32_t pa) { int16_t o; @@ -40,8 +40,8 @@ ao_pa_to_altitude(int32_t pa) o = pa >> ALT_SHIFT; part = pa & ALT_MASK; - low = (int32_t) altitude_table[o] * (ALT_SCALE - part); - high = (int32_t) altitude_table[o+1] * part + (ALT_SCALE >> 1); + low = (alt_t) FETCH_ALT(o) * (ALT_SCALE - part); + high = (alt_t) FETCH_ALT(o+1) * part + (ALT_SCALE >> 1); return (low + high) >> ALT_SHIFT; } diff --git a/src/core/ao_convert_pa_test.c b/src/core/ao_convert_pa_test.c index 972a4d4c..143ce958 100644 --- a/src/core/ao_convert_pa_test.c +++ b/src/core/ao_convert_pa_test.c @@ -17,6 +17,7 @@ #include #define AO_CONVERT_TEST +typedef int32_t alt_t; #include "ao_host.h" #include "ao_convert_pa.c" diff --git a/src/core/ao_data.h b/src/core/ao_data.h index 2b9ef5ac..90182b12 100644 --- a/src/core/ao_data.h +++ b/src/core/ao_data.h @@ -110,7 +110,12 @@ extern volatile __data uint8_t ao_data_count; #define HAS_BARO 1 typedef int32_t pres_t; -typedef int32_t alt_t; + +#ifndef AO_ALT_TYPE +#define AO_ALT_TYPE int32_t +#endif + +typedef AO_ALT_TYPE alt_t; #define ao_data_pres_cook(packet) ao_ms5607_convert(&packet->ms5607_raw, &packet->ms5607_cooked) @@ -135,6 +140,10 @@ typedef int16_t alt_t; #endif +#if !HAS_BARO +typedef int16_t alt_t; +#endif + /* * Need a few macros to pull data from the sensors: * -- cgit v1.2.3 From 16bad3b6ab65cf31b19152127cb6af69142c5c12 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 12 Oct 2012 14:03:28 -0700 Subject: altos: Include struct ao_data declaration only when used Leave it out for products that don't have a ring of sensor data Signed-off-by: Keith Packard --- src/core/ao_data.h | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'src/core/ao_data.h') diff --git a/src/core/ao_data.h b/src/core/ao_data.h index 90182b12..1153d433 100644 --- a/src/core/ao_data.h +++ b/src/core/ao_data.h @@ -52,6 +52,8 @@ #define AO_DATA_MMA655X 0 #endif +#ifdef AO_DATA_RING + #define AO_DATA_ALL (AO_DATA_ADC|AO_DATA_MS5607|AO_DATA_MPU6000|AO_DATA_HMC5883|AO_DATA_MMA655X) struct ao_data { @@ -102,6 +104,8 @@ extern volatile __data uint8_t ao_data_count; ao_sleep((void *) &ao_data_count); \ } while (0) +#endif /* AO_DATA_RING */ + #if !HAS_BARO && HAS_MS5607 /* Either an MS5607 or an MS5611 hooked to a SPI port -- cgit v1.2.3 From 5caf56e89678637c1afa79700a2fa09aa67dea9b Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 13 Oct 2012 15:02:42 -0700 Subject: altos: When missing MMA655x, create fake Z accel value This avoids overwriting the MPU6000 y acceleration value so that other computations using that value can work unmodified. Signed-off-by: Keith Packard --- src/core/ao_data.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) (limited to 'src/core/ao_data.h') diff --git a/src/core/ao_data.h b/src/core/ao_data.h index 1153d433..86acd48f 100644 --- a/src/core/ao_data.h +++ b/src/core/ao_data.h @@ -67,6 +67,9 @@ struct ao_data { #endif #if HAS_MPU6000 struct ao_mpu6000_sample mpu6000; +#if !HAS_MMA655X + int16_t z_accel; +#endif #endif #if HAS_HMC5883 struct ao_hmc5883_sample hmc5883; @@ -285,9 +288,9 @@ typedef int16_t accel_t; typedef int16_t accel_t; /* MPU6000 is hooked up so that positive y is positive acceleration */ -#define ao_data_accel(packet) ((packet)->mpu6000.accel_y) +#define ao_data_accel(packet) ((packet)->z_accel) #define ao_data_accel_cook(packet) (-(packet)->mpu6000.accel_y) -#define ao_data_set_accel(packet, accel) ((packet)->mpu6000.accel_y = (accel)) +#define ao_data_set_accel(packet, accel) ((packet)->z_accel = (accel)) #define ao_data_accel_invert(a) (-(a)) #endif -- cgit v1.2.3 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_data.h | 21 +++++++++++ src/core/ao_log.h | 14 +++++-- src/core/ao_sample.c | 95 ++++++++++++++++++++++++++++++++++++++++++++++- src/core/ao_sqrt.c | 2 + src/test/ao_flight_test.c | 17 ++++++++- 5 files changed, 142 insertions(+), 7 deletions(-) (limited to 'src/core/ao_data.h') diff --git a/src/core/ao_data.h b/src/core/ao_data.h index 6fdd19cb..7e2f85d8 100644 --- a/src/core/ao_data.h +++ b/src/core/ao_data.h @@ -288,4 +288,25 @@ typedef int16_t accel_t; #endif +#if !HAS_GYRO && HAS_MPU6000 + +#define HAS_GYRO 1 + +typedef int16_t gyro_t; +typedef int32_t angle_t; + +/* Y axis is aligned with the direction of motion (along) */ +/* X axis is aligned in the other board axis (across) */ +/* Z axis is aligned perpendicular to the board (through) */ + +#define ao_data_along(packet) ((packet)->mpu6000.accel_y) +#define ao_data_across(packet) ((packet)->mpu6000.accel_x) +#define ao_data_through(packet) ((packet)->mpu6000.accel_z) + +#define ao_data_roll(packet) ((packet)->mpu6000.gyro_y) +#define ao_data_pitch(packet) ((packet)->mpu6000.gyro_x) +#define ao_data_yaw(packet) ((packet)->mpu6000.gyro_z) + +#endif + #endif /* _AO_DATA_H_ */ diff --git a/src/core/ao_log.h b/src/core/ao_log.h index 4eaed420..93b01778 100644 --- a/src/core/ao_log.h +++ b/src/core/ao_log.h @@ -199,10 +199,16 @@ struct ao_log_mega { union { /* 4 */ /* AO_LOG_FLIGHT */ struct { - uint16_t flight; /* 4 */ - int16_t ground_accel; /* 6 */ - uint32_t ground_pres; /* 8 */ - } flight; /* 12 */ + uint16_t flight; /* 4 */ + int16_t ground_accel; /* 6 */ + uint32_t ground_pres; /* 8 */ + 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 */ + } flight; /* 24 */ /* AO_LOG_STATE */ struct { uint16_t state; 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; diff --git a/src/core/ao_sqrt.c b/src/core/ao_sqrt.c index 09c2e319..3a550eaa 100644 --- a/src/core/ao_sqrt.c +++ b/src/core/ao_sqrt.c @@ -15,7 +15,9 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. */ +#ifndef AO_FLIGHT_TEST #include "ao.h" +#endif /* Adapted from int_sqrt.c in the linux kernel, which is licensed GPLv2 */ /** diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index acdf4d92..cdd1f236 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -236,10 +236,14 @@ extern int32_t ao_accel_scale; extern alt_t ao_ground_height; extern alt_t ao_sample_alt; +double ao_sample_qangle; + int ao_sample_prev_tick; uint16_t prev_tick; + #include "ao_kalman.c" +#include "ao_sqrt.c" #include "ao_sample.c" #include "ao_flight.c" @@ -309,7 +313,7 @@ ao_mpu6000_accel(int16_t sensor) } static double -ao_mpu6000_gyro(int16_t sensor) +ao_mpu6000_gyro(int32_t sensor) { return sensor / 32767.0 * MPU6000_GYRO_FULLSCALE; } @@ -370,6 +374,7 @@ ao_insert(void) if (!ao_summary) { printf("%7.2f height %8.2f accel %8.3f " #if MEGAMETRUM + "roll %8.3f angle %8.3f qangle %8.3f " "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f " #endif "state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d\n", @@ -377,6 +382,9 @@ ao_insert(void) height, accel, #if MEGAMETRUM + ao_mpu6000_gyro(ao_sample_roll_angle) / 100.0, + ao_mpu6000_gyro(ao_sample_angle) / 100.0, + ao_sample_qangle, ao_mpu6000_accel(ao_data_static.mpu6000.accel_x), ao_mpu6000_accel(ao_data_static.mpu6000.accel_y), ao_mpu6000_accel(ao_data_static.mpu6000.accel_z), @@ -715,12 +723,14 @@ update_orientation (double rate_x, double rate_y, double rate_z, int tick) q_dot.q2 = 0.5 * (ao_orient.q0 * rate_y + ao_orient.q3 * rate_x - ao_orient.q1 * rate_z) + lambda * ao_orient.q2; q_dot.q3 = 0.5 * (ao_orient.q0 * rate_z + ao_orient.q1 * rate_y - ao_orient.q2 * rate_x) + lambda * ao_orient.q3; +#if 0 printf ("update_orientation %g %g %g (%g s)\n", rate_x, rate_y, rate_z, dt); printf ("q_dot (%g) %g %g %g\n", q_dot.q0, q_dot.q1, q_dot.q2, q_dot.q3); +#endif ao_orient.q0 += q_dot.q0 * dt; ao_orient.q1 += q_dot.q1 * dt; @@ -731,6 +741,8 @@ update_orientation (double rate_x, double rate_y, double rate_z, int tick) ao_quat_rot(&ao_current, &ao_up, &ao_orient); + ao_sample_qangle = 180 / M_PI * acos(ao_current.q3 * sqrt(2)); +#if 0 printf ("orient (%g) %g %g %g current (%g) %g %g %g\n", ao_orient.q0, ao_orient.q1, @@ -740,6 +752,7 @@ update_orientation (double rate_x, double rate_y, double rate_z, int tick) ao_current.q1, ao_current.q2, ao_current.q3); +#endif } #endif @@ -845,7 +858,7 @@ ao_sleep(void *wchan) double rate_y = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y); double rate_z = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z); - update_orientation(rate_x, rate_z, rate_y, tick); + update_orientation(rate_x * M_PI / 180, rate_z * M_PI / 180, rate_y * M_PI / 180, tick); } ao_records_read++; ao_insert(); -- cgit v1.2.3