From 0571531066918fdefe9447f3b4192d0c6c477afa Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 14 May 2013 10:48:24 -0700 Subject: altos: Grab SPI mutex until MPU6000 I2C mode is disabled If other drivers use the SPI bus, the MPU6000 gets confused as its sitting on the bus looking for I2C messages. Just grab the mutex before the OS is running and hold onto it until the MPU6000 has been initialized. Signed-off-by: Keith Packard --- src/drivers/ao_mpu6000.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) (limited to 'src/drivers/ao_mpu6000.h') diff --git a/src/drivers/ao_mpu6000.h b/src/drivers/ao_mpu6000.h index f01e9e83..a42f69c4 100644 --- a/src/drivers/ao_mpu6000.h +++ b/src/drivers/ao_mpu6000.h @@ -133,13 +133,13 @@ #define MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET 1 #define MPU6000_SIGNAL_PATH_RESET_TEMP_RESET 0 -#define MPU6000_USER_CONTROL 0x6a -#define MPU6000_USER_CONTROL_FIFO_EN 6 -#define MPU6000_USER_CONTROL_I2C_MST_EN 5 -#define MPU6000_USER_CONTROL_I2C_IF_DIS 4 -#define MPU6000_USER_CONTROL_FIFO_RESET 2 -#define MPU6000_USER_CONTROL_I2C_MST_RESET 1 -#define MPU6000_USER_CONTROL_SIG_COND_RESET 0 +#define MPU6000_USER_CTRL 0x6a +#define MPU6000_USER_CTRL_FIFO_EN 6 +#define MPU6000_USER_CTRL_I2C_MST_EN 5 +#define MPU6000_USER_CTRL_I2C_IF_DIS 4 +#define MPU6000_USER_CTRL_FIFO_RESET 2 +#define MPU6000_USER_CTRL_I2C_MST_RESET 1 +#define MPU6000_USER_CTRL_SIG_COND_RESET 0 #define MPU6000_PWR_MGMT_1 0x6b #define MPU6000_PWR_MGMT_1_DEVICE_RESET 7 -- cgit v1.2.3 From 08143a922fe27bc50a19924f46538f9476ab5fd1 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 25 Oct 2013 04:05:09 -0700 Subject: altos: Add gyro-based orientation tracking This tracks the angle-from-vertical as an additional input to the pyro channels. Signed-off-by: Keith Packard --- src/core/ao_data.h | 6 +- src/core/ao_flight.c | 4 + src/core/ao_kalman.c | 3 - src/core/ao_pyro.c | 4 +- src/core/ao_quaternion.h | 116 +++++++++++++++++++ src/core/ao_sample.c | 98 +++++++++++++++- src/core/ao_sample.h | 3 +- src/core/ao_telemetry.c | 1 + src/core/ao_telemetry.h | 2 +- src/drivers/ao_mpu6000.c | 2 + src/drivers/ao_mpu6000.h | 17 ++- src/stm/Makefile.defs | 4 +- src/telemega-v0.3/Makefile | 16 +++ src/test/Makefile | 9 +- src/test/ao_flight_test.c | 259 ++---------------------------------------- src/test/ao_quaternion_test.c | 67 +++++++++++ 16 files changed, 343 insertions(+), 268 deletions(-) create mode 100644 src/core/ao_quaternion.h create mode 100644 src/test/ao_quaternion_test.c (limited to 'src/drivers/ao_mpu6000.h') diff --git a/src/core/ao_data.h b/src/core/ao_data.h index 339afe69..5a232885 100644 --- a/src/core/ao_data.h +++ b/src/core/ao_data.h @@ -18,6 +18,8 @@ #ifndef _AO_DATA_H_ #define _AO_DATA_H_ +#define GRAVITY 9.80665 + #if HAS_ADC #define AO_DATA_ADC (1 << 0) #else @@ -300,8 +302,8 @@ typedef int16_t accel_t; #define HAS_GYRO 1 -typedef int16_t gyro_t; -typedef int32_t angle_t; +typedef int16_t gyro_t; /* in raw sample units */ +typedef int16_t angle_t; /* in degrees */ /* Y axis is aligned with the direction of motion (along) */ /* X axis is aligned in the other board axis (across) */ diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index 2495a44b..240b348a 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -20,6 +20,10 @@ #include #endif +#if HAS_MPU6000 +#include +#endif + #ifndef HAS_ACCEL #error Please define HAS_ACCEL #endif diff --git a/src/core/ao_kalman.c b/src/core/ao_kalman.c index 762b2c0a..7fd4f889 100644 --- a/src/core/ao_kalman.c +++ b/src/core/ao_kalman.c @@ -292,7 +292,4 @@ ao_kalman(void) else #endif ao_avg_height = (ao_avg_height_scaled + 63) >> 7; -#ifdef AO_FLIGHT_TEST - ao_sample_prev_tick = ao_sample_tick; -#endif } diff --git a/src/core/ao_pyro.c b/src/core/ao_pyro.c index 531e1f50..24c9fe99 100644 --- a/src/core/ao_pyro.c +++ b/src/core/ao_pyro.c @@ -113,7 +113,7 @@ ao_pyro_ready(struct ao_pyro *pyro) continue; break; -#if HAS_ORIENT +#if HAS_GYRO case ao_pyro_orient_less: if (ao_orient <= pyro->orient_less) continue; @@ -310,7 +310,7 @@ const struct { { "h<", ao_pyro_height_less, offsetof(struct ao_pyro, height_less), HELP("height less (m)") }, { "h>", ao_pyro_height_greater, offsetof(struct ao_pyro, height_greater), HELP("height greater (m)") }, -#if HAS_ORIENT +#if HAS_GYRO { "o<", ao_pyro_orient_less, offsetof(struct ao_pyro, orient_less), HELP("orient less (deg)") }, { "o>", ao_pyro_orient_greater, offsetof(struct ao_pyro, orient_greater), HELP("orient greater (deg)") }, #endif diff --git a/src/core/ao_quaternion.h b/src/core/ao_quaternion.h new file mode 100644 index 00000000..f4b8aaa4 --- /dev/null +++ b/src/core/ao_quaternion.h @@ -0,0 +1,116 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_QUATERNION_H_ +#define _AO_QUATERNION_H_ + +#include + +struct ao_quaternion { + float r; /* real bit */ + float x, y, z; /* imaginary bits */ +}; + +static inline void ao_quaternion_multiply(struct ao_quaternion *r, + struct ao_quaternion *a, + struct ao_quaternion *b) +{ + struct ao_quaternion t; +#define T(_a,_b) (((a)->_a) * ((b)->_b)) + t.r = T(r,r) - T(x,x) - T(y,y) - T(z,z); + t.x = T(r,x) + T(x,r) + T(y,z) - T(z,y); + t.y = T(r,y) - T(x,z) + T(y,r) + T(z,x); + t.z = T(r,z) + T(x,y) - T(y,x) + T(z,r); +#undef T + *r = t; +} + +static inline void ao_quaternion_conjugate(struct ao_quaternion *r, + struct ao_quaternion *a) +{ + r->r = a->r; + r->x = -a->x; + r->y = -a->y; + r->z = -a->z; +} + +static inline float ao_quaternion_normal(struct ao_quaternion *a) +{ +#define S(_a) (((a)->_a) * ((a)->_a)) + return S(r) + S(x) + S(y) + S(z); +#undef S +} + +static inline void ao_quaternion_scale(struct ao_quaternion *r, + struct ao_quaternion *a, + float b) +{ + r->r = a->r * b; + r->x = a->x * b; + r->y = a->y * b; + r->z = a->z * b; +} + +static inline void ao_quaternion_normalize(struct ao_quaternion *r, + struct ao_quaternion *a) +{ + float n = ao_quaternion_normal(a); + + if (n > 0) + ao_quaternion_scale(r, a, 1/sqrtf(n)); + else + *r = *a; +} + +static inline void ao_quaternion_rotate(struct ao_quaternion *r, + struct ao_quaternion *a, + struct ao_quaternion *b) +{ + struct ao_quaternion c; + struct ao_quaternion t; + + ao_quaternion_conjugate(&c, b); + ao_quaternion_multiply(&t, b, a); + ao_quaternion_multiply(r, &t, &c); +} + +static inline void ao_quaternion_init_vector(struct ao_quaternion *r, + float x, float y, float z) +{ + r->r = 0; + r->x = x; + r->y = y; + r->z = z; +} + +static inline void ao_quaternion_init_rotation(struct ao_quaternion *r, + float x, float y, float z, + float s, float c) +{ + r->r = c; + r->x = s * x; + r->y = s * y; + r->z = s * z; +} + +static inline void ao_quaternion_init_zero_rotation(struct ao_quaternion *r) +{ + r->r = 1; + r->x = r->y = r->z = 0; +} + +#endif /* _AO_QUATERNION_H_ */ diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c index dec44f9f..676e0ffd 100644 --- a/src/core/ao_sample.c +++ b/src/core/ao_sample.c @@ -20,6 +20,10 @@ #include #endif +#if HAS_GYRO +#include +#endif + /* * Current sensor values */ @@ -44,8 +48,7 @@ __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; +__pdata angle_t ao_orient; #endif __data uint8_t ao_sample_data; @@ -86,6 +89,8 @@ __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; +static struct ao_quaternion ao_rotation; +static struct ao_quaternion ao_pad_orientation; #endif static void @@ -129,11 +134,91 @@ ao_sample_preflight_set(void) ao_sample_pitch_sum = 0; ao_sample_yaw_sum = 0; ao_sample_roll_sum = 0; - ao_sample_angle = 0; + ao_orient = 0; + + /* No rotation yet */ + ao_quaternion_init_zero_rotation(&ao_rotation); + + /* XXX Assume we're pointing straight up for now */ + ao_quaternion_init_vector(&ao_pad_orientation, + ao_ground_accel_across, + ao_ground_accel_through, + -ao_ground_accel_along); + ao_quaternion_normalize(&ao_pad_orientation, + &ao_pad_orientation); + + printf ("pad r%8.5f x%8.5f y%8.5f z%8.5f\n", + ao_pad_orientation.r, + ao_pad_orientation.x, + ao_pad_orientation.y, + ao_pad_orientation.z); #endif nsamples = 0; } +#if HAS_GYRO +static void +ao_sample_rotate(void) +{ +#ifdef AO_FLIGHT_TEST + float dt = (ao_sample_tick - ao_sample_prev_tick) / 100.0; +#else + static const float dt = 1/100.0; +#endif + float x = ao_mpu6000_gyro(ao_sample_pitch - ao_ground_pitch) * dt; + float y = ao_mpu6000_gyro(ao_sample_yaw - ao_ground_yaw) * dt; + float z = ao_mpu6000_gyro(ao_sample_roll - ao_ground_roll) * dt; + + float n_2, n; + float s, c; + + struct ao_quaternion rot; + struct ao_quaternion point; + + /* The amount of rotation is just the length of the vector. Now, + * here's the trick -- assume that the rotation amount is small. In this case, + * sin(x) ≃ x, so we can just make this the sin. + */ + + n_2 = x*x + y*y + z*z; + n = sqrtf(n_2); + s = n / 2; + if (s > 1) + s = 1; + c = sqrtf(1 - s*s); + + /* Make unit vector */ + if (n > 0) { + x /= n; + y /= n; + z /= n; + } + + /* Now compute the unified rotation quaternion */ + + ao_quaternion_init_rotation(&rot, + x, y, z, + s, c); + + /* Integrate with the previous rotation amount */ + ao_quaternion_multiply(&ao_rotation, &ao_rotation, &rot); + + /* And normalize to make sure it remains a unit vector */ + ao_quaternion_normalize(&ao_rotation, &ao_rotation); + + /* Compute pitch angle from vertical by taking the pad + * orientation vector and rotating it by the current total + * rotation value. That will be a unit vector pointing along + * the airframe axis. The Z value will be the cosine of the + * change in the angle from vertical since boost + */ + + ao_quaternion_rotate(&point, &ao_pad_orientation, &ao_rotation); + + ao_orient = acosf(point.z) * (float) (180.0/M_PI); +} +#endif + static void ao_sample_preflight(void) { @@ -232,9 +317,12 @@ ao_sample(void) ao_sample_preflight_update(); ao_kalman(); #if HAS_GYRO - /* do quaternion stuff here... */ + ao_sample_rotate(); #endif } +#ifdef AO_FLIGHT_TEST + ao_sample_prev_tick = ao_sample_tick; +#endif ao_sample_data = ao_data_ring_next(ao_sample_data); } return !ao_preflight; @@ -264,7 +352,7 @@ ao_sample_init(void) ao_sample_pitch = 0; ao_sample_yaw = 0; ao_sample_roll = 0; - ao_sample_angle = 0; + ao_orient = 0; #endif ao_sample_data = ao_data_head; ao_preflight = TRUE; diff --git a/src/core/ao_sample.h b/src/core/ao_sample.h index 5bd29536..1d1bcc7c 100644 --- a/src/core/ao_sample.h +++ b/src/core/ao_sample.h @@ -64,8 +64,6 @@ * for all further flight computations */ -#define GRAVITY 9.80665 - /* * Above this height, the baro sensor doesn't work */ @@ -118,6 +116,7 @@ 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; +extern __pdata angle_t ao_orient; #endif void ao_sample_init(void); diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c index 6b47a06a..a2726016 100644 --- a/src/core/ao_telemetry.c +++ b/src/core/ao_telemetry.c @@ -115,6 +115,7 @@ ao_send_mega_sensor(void) telemetry.generic.tick = packet->tick; telemetry.generic.type = AO_TELEMETRY_MEGA_SENSOR; + telemetry.mega_sensor.orient = ao_orient; telemetry.mega_sensor.accel = ao_data_accel(packet); telemetry.mega_sensor.pres = ao_data_pres(packet); telemetry.mega_sensor.temp = ao_data_temp(packet); diff --git a/src/core/ao_telemetry.h b/src/core/ao_telemetry.h index fd6b76b3..237a35ab 100644 --- a/src/core/ao_telemetry.h +++ b/src/core/ao_telemetry.h @@ -162,7 +162,7 @@ struct ao_telemetry_mega_sensor { uint16_t tick; /* 2 */ uint8_t type; /* 4 */ - uint8_t pad5; /* 5 */ + uint8_t orient; /* 5 angle from vertical */ int16_t accel; /* 6 Z axis */ int32_t pres; /* 8 Pa * 10 */ diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c index 73057820..5e75b78a 100644 --- a/src/drivers/ao_mpu6000.c +++ b/src/drivers/ao_mpu6000.c @@ -118,6 +118,7 @@ _ao_mpu6000_sample(struct ao_mpu6000_sample *sample) #define G 981 /* in cm/s² */ +#if 0 static int16_t /* cm/s² */ ao_mpu6000_accel(int16_t v) { @@ -129,6 +130,7 @@ ao_mpu6000_gyro(int16_t v) { return (int16_t) ((v * (int32_t) 20000) / 32767); } +#endif static uint8_t ao_mpu6000_accel_check(int16_t normal, int16_t test, char *which) diff --git a/src/drivers/ao_mpu6000.h b/src/drivers/ao_mpu6000.h index a42f69c4..2241bf80 100644 --- a/src/drivers/ao_mpu6000.h +++ b/src/drivers/ao_mpu6000.h @@ -18,6 +18,10 @@ #ifndef _AO_MPU6000_H_ #define _AO_MPU6000_H_ +#ifndef M_PI +#define M_PI 3.1415926535897832384626433 +#endif + #define MPU6000_ADDR_WRITE 0xd0 #define MPU6000_ADDR_READ 0xd1 @@ -166,9 +170,20 @@ /* Self test gyro is approximately 50°/s */ #define MPU6000_ST_GYRO(full_scale) ((int16_t) (((int32_t) 32767 * (int32_t) 50) / (full_scale))) -#define MPU6000_GYRO_FULLSCALE 2000 +#define MPU6000_GYRO_FULLSCALE ((float) 2000 * M_PI/180.0) + +static inline float +ao_mpu6000_gyro(int16_t sensor) { + return (float) sensor * ((float) (MPU6000_GYRO_FULLSCALE / 32767.0)); +} + #define MPU6000_ACCEL_FULLSCALE 16 +static inline float +ao_mpu6000_accel(int16_t sensor) { + return (float) sensor * ((float) (MPU6000_ACCEL_FULLSCALE * GRAVITY / 32767.0)); +} + struct ao_mpu6000_sample { int16_t accel_x; int16_t accel_y; diff --git a/src/stm/Makefile.defs b/src/stm/Makefile.defs index ee1cb658..0710d747 100644 --- a/src/stm/Makefile.defs +++ b/src/stm/Makefile.defs @@ -1,4 +1,4 @@ -vpath % ../stm:../product:../drivers:../core:../util:../kalman:../aes:.. +vpath % ../stm:../product:../drivers:../core:../util:../kalman:../aes:../math:.. vpath make-altitude ../util vpath make-kalman ../util vpath kalman.5c ../kalman @@ -24,7 +24,7 @@ include $(TOPDIR)/Makedefs CC=$(ARM_CC) LIBS=-lpdclib-cortex-m3 -lgcc -AO_CFLAGS=-I. -I../stm -I../core -I../drivers -I.. +AO_CFLAGS=-I. -I../stm -I../core -I../drivers -I../math -I.. STM_CFLAGS=-std=gnu99 -mlittle-endian -mcpu=cortex-m3 -mthumb -ffreestanding -nostdlib $(AO_CFLAGS) LDFLAGS=-L../stm -Wl,-Taltos.ld diff --git a/src/telemega-v0.3/Makefile b/src/telemega-v0.3/Makefile index 6e5da721..2f460105 100644 --- a/src/telemega-v0.3/Makefile +++ b/src/telemega-v0.3/Makefile @@ -27,6 +27,7 @@ INC = \ ao_sample_profile.h \ ao_mpu.h \ stm32l.h \ + math.h \ Makefile # @@ -44,6 +45,20 @@ INC = \ #STACK_GUARD=ao_mpu_stm.c #STACK_GUARD_DEF=-DHAS_STACK_GUARD=1 +MATH_SRC=\ + ef_acos.c \ + ef_sqrt.c + +# ef_rem_pio2.c \ +# kf_cos.c \ +# kf_sin.c \ +# kf_rem_pio2.c \ +# sf_copysign.c \ +# sf_cos.c \ +# sf_fabs.c \ +# sf_floor.c \ +# sf_scalbn.c + ALTOS_SRC = \ ao_boot_chain.c \ ao_interrupt.c \ @@ -93,6 +108,7 @@ ALTOS_SRC = \ ao_companion.c \ ao_pyro.c \ ao_aprs.c \ + $(MATH_SRC) \ $(PROFILE) \ $(SAMPLE_PROFILE) \ $(STACK_GUARD) diff --git a/src/test/Makefile b/src/test/Makefile index 5eee6bbb..f64a9531 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -1,15 +1,15 @@ -vpath % ..:../core:../drivers:../util:../micropeak:../aes +vpath % ..:../core:../drivers:../util:../micropeak:../aes:../product PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_flight_test_mm \ ao_gps_test ao_gps_test_skytraq ao_gps_test_ublox ao_convert_test ao_convert_pa_test ao_fec_test \ ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test ao_int64_test \ - ao_ms5607_convert_test + ao_ms5607_convert_test ao_quaternion_test INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h KALMAN=make-kalman -CFLAGS=-I.. -I. -I../core -I../drivers -I../micropeak -O0 -g -Wall +CFLAGS=-I.. -I. -I../core -I../drivers -I../micropeak -I../product -O0 -g -Wall all: $(PROGS) ao_aprs_data.wav @@ -80,3 +80,6 @@ ao_int64_test: ao_int64_test.c ao_int64.c ao_int64.h ao_ms5607_convert_test: ao_ms5607_convert_test.c ao_ms5607_convert_8051.c ao_int64.c ao_int64.h cc $(CFLAGS) -o $@ ao_ms5607_convert_test.c + +ao_quaternion_test: ao_quaternion_test.c ao_quaternion.h + cc $(CFLAGS) -o $@ ao_quaternion_test.c -lm \ No newline at end of file diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index faf31aa7..e2f63e34 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -25,6 +25,8 @@ #include #include +#define GRAVITY 9.80665 + #define AO_HERTZ 100 #define HAS_ADC 1 @@ -36,6 +38,11 @@ #define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16)) #define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16)) +#define AO_GPS_NEW_DATA 1 +#define AO_GPS_NEW_TRACKING 2 + +int ao_gps_new; + #if TELEMEGA #define AO_ADC_NUM_SENSE 6 #define HAS_MS5607 1 @@ -240,7 +247,6 @@ struct ao_config ao_config; #define DATA_TO_XDATA(x) (x) -#define GRAVITY 9.80665 extern int16_t ao_ground_accel, ao_flight_accel; extern int16_t ao_accel_2g; @@ -339,20 +345,6 @@ ao_test_exit(void) exit(0); } -#if HAS_MPU6000 -static double -ao_mpu6000_accel(int16_t sensor) -{ - return sensor / 32767.0 * MPU6000_ACCEL_FULLSCALE * GRAVITY; -} - -static double -ao_mpu6000_gyro(int32_t sensor) -{ - return sensor / 32767.0 * MPU6000_GYRO_FULLSCALE; -} -#endif - void ao_insert(void) { @@ -408,23 +400,23 @@ ao_insert(void) if (!ao_summary) { printf("%7.2f height %8.2f accel %8.3f " #if TELEMEGA - "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 " + "angle %5d " +/* "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", time, height, accel, #if TELEMEGA - ao_mpu6000_gyro(ao_sample_roll_angle) / 100.0, - ao_mpu6000_gyro(ao_sample_angle) / 100.0, - ao_sample_qangle, + ao_orient, +/* 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), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z), +*/ #endif ao_state_names[ao_flight_state], ao_k_height / 65536.0, @@ -589,207 +581,6 @@ int32(uint8_t *bytes, int off) static int log_format; -#if TELEMEGA - -static double -ao_vec_norm(double x, double y, double z) -{ - return x*x + y*y + z*z; -} - -static void -ao_vec_normalize(double *x, double *y, double *z) -{ - double scale = 1/sqrt(ao_vec_norm(*x, *y, *z)); - - *x *= scale; - *y *= scale; - *z *= scale; -} - -struct ao_quat { - double q0, q1, q2, q3; -}; - -static void -ao_quat_mul(struct ao_quat *r, struct ao_quat *a, struct ao_quat *b) -{ - r->q0 = a->q0 * b->q0 - a->q1 * b->q1 - a->q2 * b->q2 - a->q3 * b->q3; - r->q1 = a->q0 * b->q1 + a->q1 * b->q0 + a->q2 * b->q3 - a->q3 * b->q2; - r->q2 = a->q0 * b->q2 - a->q1 * b->q3 + a->q2 * b->q0 + a->q3 * b->q1; - r->q3 = a->q0 * b->q3 + a->q1 * b->q2 - a->q2 * b->q1 + a->q3 * b->q0; -} - -#if 0 -static void -ao_quat_scale(struct ao_quat *r, struct ao_quat *a, double s) -{ - r->q0 = a->q0 * s; - r->q1 = a->q1 * s; - r->q2 = a->q2 * s; - r->q3 = a->q3 * s; -} -#endif - -static void -ao_quat_conj(struct ao_quat *r, struct ao_quat *a) -{ - r->q0 = a->q0; - r->q1 = -a->q1; - r->q2 = -a->q2; - r->q3 = -a->q3; -} - -static void -ao_quat_rot(struct ao_quat *r, struct ao_quat *a, struct ao_quat *q) -{ - struct ao_quat t; - struct ao_quat c; - ao_quat_mul(&t, q, a); - ao_quat_conj(&c, q); - ao_quat_mul(r, &t, &c); -} - -static void -ao_quat_from_angle(struct ao_quat *r, - double x_rad, - double y_rad, - double z_rad) -{ - double angle = sqrt (x_rad * x_rad + y_rad * y_rad + z_rad * z_rad); - double s = sin(angle/2); - double c = cos(angle/2); - - r->q0 = c; - r->q1 = x_rad * s / angle; - r->q2 = y_rad * s / angle; - r->q3 = z_rad * s / angle; -} - -static void -ao_quat_from_vector(struct ao_quat *r, double x, double y, double z) -{ - ao_vec_normalize(&x, &y, &z); - double x_rad = atan2(z, y); - double y_rad = atan2(x, z); - double z_rad = atan2(y, x); - - ao_quat_from_angle(r, x_rad, y_rad, z_rad); -} - -static double -ao_quat_norm(struct ao_quat *a) -{ - return (a->q0 * a->q0 + - a->q1 * a->q1 + - a->q2 * a->q2 + - a->q3 * a->q3); -} - -static void -ao_quat_normalize(struct ao_quat *a) -{ - double norm = ao_quat_norm(a); - - if (norm) { - double m = 1/sqrt(norm); - - a->q0 *= m; - a->q1 *= m; - a->q2 *= m; - a->q3 *= m; - } -} - -static struct ao_quat ao_up, ao_current; -static struct ao_quat ao_orient; -static int ao_orient_tick; - -void -set_orientation(double x, double y, double z, int tick) -{ - struct ao_quat t; - - printf ("set_orientation %g %g %g\n", x, y, z); - ao_quat_from_vector(&ao_orient, x, y, z); - ao_up.q1 = ao_up.q2 = 0; - ao_up.q0 = ao_up.q3 = sqrt(2)/2; - ao_orient_tick = tick; - - ao_orient.q0 = 1; - ao_orient.q1 = 0; - ao_orient.q2 = 0; - ao_orient.q3 = 0; - - printf ("orient (%g) %g %g %g up (%g) %g %g %g\n", - ao_orient.q0, - ao_orient.q1, - ao_orient.q2, - ao_orient.q3, - ao_up.q0, - ao_up.q1, - ao_up.q2, - ao_up.q3); - - ao_quat_rot(&t, &ao_up, &ao_orient); - printf ("pad orient (%g) %g %g %g\n", - t.q0, - t.q1, - t.q2, - t.q3); - -} - -void -update_orientation (double rate_x, double rate_y, double rate_z, int tick) -{ - struct ao_quat q_dot; - double lambda; - double dt = (tick - ao_orient_tick) / 100.0; - - ao_orient_tick = tick; - -// lambda = 1 - ao_quat_norm(&ao_orient); - lambda = 0; - - q_dot.q0 = -0.5 * (ao_orient.q1 * rate_x + ao_orient.q2 * rate_y + ao_orient.q3 * rate_z) + lambda * ao_orient.q0; - q_dot.q1 = 0.5 * (ao_orient.q0 * rate_x + ao_orient.q2 * rate_z - ao_orient.q3 * rate_y) + lambda * ao_orient.q1; - 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; - ao_orient.q2 += q_dot.q2 * dt; - ao_orient.q3 += q_dot.q3 * dt; - - ao_quat_normalize(&ao_orient); - - 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, - ao_orient.q2, - ao_orient.q3, - ao_current.q0, - ao_current.q1, - ao_current.q2, - ao_current.q3); -#endif -} -#endif - void ao_sleep(void *wchan) { @@ -872,32 +663,6 @@ ao_sleep(void *wchan) #if HAS_MMA655X ao_data_static.mma655x = int16(bytes, 26); #endif - if (ao_records_read == 0) - ao_ground_mpu6000 = ao_data_static.mpu6000; - else if (ao_records_read < 10) { -#define f(f) ao_ground_mpu6000.f = ao_ground_mpu6000.f + ((ao_data_static.mpu6000.f - ao_ground_mpu6000.f) >> 2) - f(accel_x); - f(accel_y); - f(accel_z); - f(gyro_x); - f(gyro_y); - f(gyro_z); - - double accel_x = ao_mpu6000_accel(ao_ground_mpu6000.accel_x); - double accel_y = ao_mpu6000_accel(ao_ground_mpu6000.accel_y); - double accel_z = ao_mpu6000_accel(ao_ground_mpu6000.accel_z); - - /* X and Y are in the ground plane, arbitraryily picked as MPU X and Z axes - * Z is normal to the ground, the MPU y axis - */ - set_orientation(accel_x, accel_z, accel_y, tick); - } else { - double rate_x = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x); - 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 * M_PI / 180, rate_z * M_PI / 180, rate_y * M_PI / 180, tick); - } ao_records_read++; ao_insert(); return; diff --git a/src/test/ao_quaternion_test.c b/src/test/ao_quaternion_test.c new file mode 100644 index 00000000..0e4b5b07 --- /dev/null +++ b/src/test/ao_quaternion_test.c @@ -0,0 +1,67 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#define _GNU_SOURCE + +#include +#include +#include +#include +#include +#include +#include + +#include "ao_quaternion.h" + +static void +print_q(char *name, struct ao_quaternion *q) +{ + printf ("%8.8s: r%8.5f x%8.5f y%8.5f z%8.5f ", name, + q->r, q->x, q->y, q->z); +} + +int main(int argc, char **argv) +{ + struct ao_quaternion position; + struct ao_quaternion rotation; + struct ao_quaternion little_rotation; + int i; + + /* unit x vector */ + ao_quaternion_init_vector(&position, 1, 0, 0); + + /* zero rotation */ + ao_quaternion_init_zero_rotation(&rotation); + + /* π/16 rotation around Z axis */ + ao_quaternion_init_rotation(&little_rotation, 0, 0, 1, + sin((M_PI/16)/2), + cos((M_PI/16)/2)); + for (i = 0; i <= 16; i++) { + struct ao_quaternion rotated; + + ao_quaternion_rotate(&rotated, &position, &rotation); + print_q("position", &position); + print_q("rotated", &rotated); + print_q("rotation", &rotation); + printf ("\n"); + ao_quaternion_multiply(&rotation, &rotation, &little_rotation); + ao_quaternion_normalize(&rotation, &rotation); + } + return 0; +} + -- cgit v1.2.3 From 195fd70cdc7f519cd8d4ac323088ed0b6c188280 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 27 Oct 2013 23:42:58 -0700 Subject: altos: Change ao_mpu6000_gyro arg to float This lets callers pass more precision than just the original sensor value Signed-off-by: Keith Packard --- src/drivers/ao_mpu6000.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/drivers/ao_mpu6000.h') diff --git a/src/drivers/ao_mpu6000.h b/src/drivers/ao_mpu6000.h index 2241bf80..dc3a9fbf 100644 --- a/src/drivers/ao_mpu6000.h +++ b/src/drivers/ao_mpu6000.h @@ -173,8 +173,8 @@ #define MPU6000_GYRO_FULLSCALE ((float) 2000 * M_PI/180.0) static inline float -ao_mpu6000_gyro(int16_t sensor) { - return (float) sensor * ((float) (MPU6000_GYRO_FULLSCALE / 32767.0)); +ao_mpu6000_gyro(float sensor) { + return sensor * ((float) (MPU6000_GYRO_FULLSCALE / 32767.0)); } #define MPU6000_ACCEL_FULLSCALE 16 -- cgit v1.2.3