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_quaternion.h | 116 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 116 insertions(+) create mode 100644 src/core/ao_quaternion.h (limited to 'src/core/ao_quaternion.h') 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_ */ -- cgit v1.2.3 From 616977d2955da13383a1869b9ccdb07338172109 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 27 Oct 2013 23:10:13 -0700 Subject: altos: Mark arguments to quaternion functions as const Lets us pass constants without the compile whinging Signed-off-by: Keith Packard --- src/core/ao_quaternion.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'src/core/ao_quaternion.h') diff --git a/src/core/ao_quaternion.h b/src/core/ao_quaternion.h index f4b8aaa4..1c0617c4 100644 --- a/src/core/ao_quaternion.h +++ b/src/core/ao_quaternion.h @@ -26,8 +26,8 @@ struct ao_quaternion { }; static inline void ao_quaternion_multiply(struct ao_quaternion *r, - struct ao_quaternion *a, - struct ao_quaternion *b) + const struct ao_quaternion *a, + const struct ao_quaternion *b) { struct ao_quaternion t; #define T(_a,_b) (((a)->_a) * ((b)->_b)) @@ -40,7 +40,7 @@ static inline void ao_quaternion_multiply(struct ao_quaternion *r, } static inline void ao_quaternion_conjugate(struct ao_quaternion *r, - struct ao_quaternion *a) + const struct ao_quaternion *a) { r->r = a->r; r->x = -a->x; @@ -48,7 +48,7 @@ static inline void ao_quaternion_conjugate(struct ao_quaternion *r, r->z = -a->z; } -static inline float ao_quaternion_normal(struct ao_quaternion *a) +static inline float ao_quaternion_normal(const struct ao_quaternion *a) { #define S(_a) (((a)->_a) * ((a)->_a)) return S(r) + S(x) + S(y) + S(z); @@ -56,7 +56,7 @@ static inline float ao_quaternion_normal(struct ao_quaternion *a) } static inline void ao_quaternion_scale(struct ao_quaternion *r, - struct ao_quaternion *a, + const struct ao_quaternion *a, float b) { r->r = a->r * b; @@ -66,7 +66,7 @@ static inline void ao_quaternion_scale(struct ao_quaternion *r, } static inline void ao_quaternion_normalize(struct ao_quaternion *r, - struct ao_quaternion *a) + const struct ao_quaternion *a) { float n = ao_quaternion_normal(a); -- cgit v1.2.3 From 3b25860b5b3b69642928dd9c30dec4b4b937a88c Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 27 Oct 2013 23:11:09 -0700 Subject: altos: Add some comments describing quaternion multiplication Signed-off-by: Keith Packard --- src/core/ao_quaternion.h | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) (limited to 'src/core/ao_quaternion.h') diff --git a/src/core/ao_quaternion.h b/src/core/ao_quaternion.h index 1c0617c4..6a701a18 100644 --- a/src/core/ao_quaternion.h +++ b/src/core/ao_quaternion.h @@ -31,6 +31,39 @@ static inline void ao_quaternion_multiply(struct ao_quaternion *r, { struct ao_quaternion t; #define T(_a,_b) (((a)->_a) * ((b)->_b)) + +/* + * Quaternions + * + * ii = jj = kk = ijk = -1; + * + * kji = 1; + * + * ij = k; ji = -k; + * kj = -i; jk = i; + * ik = -j; ki = j; + * + * Multiplication p * q: + * + * (pr + ipx + jpy + kpz) (qr + iqx + jqy + kqz) = + * + * ( pr * qr + pr * iqx + pr * jqy + pr * kqz) + + * (ipx * qr + ipx * iqx + ipx * jqy + ipx * kqz) + + * (jpy * qr + jpy * iqx + jpy * jqy + jpy * kqz) + + * (kpz * qr + kpz * iqx + kpz * jqy + kpz * kqz) = + * + * + * (pr * qr) + i(pr * qx) + j(pr * qy) + k(pr * qz) + + * i(px * qr) - (px * qx) + k(px * qy) - j(px * qz) + + * j(py * qr) - k(py * qx) - (py * qy) + i(py * qz) + + * k(pz * qr) + j(pz * qx) - i(pz * qy) - (pz * qz) = + * + * 1 * ( (pr * qr) - (px * qx) - (py * qy) - (pz * qz) ) + + * i * ( (pr * qx) + (px * qr) + (py * qz) - (pz * qy) ) + + * j * ( (pr * qy) - (px * qz) + (py * qr) + (pz * qx) ) + + * k * ( (pr * qz) + (px * qy) - (py * qx) + (pz * qr); + */ + 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); -- cgit v1.2.3 From c10cb9d31765e6ef0ba737bc484c5aed22a332f9 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 27 Oct 2013 23:11:37 -0700 Subject: altos: Add functions to init quaternions from vector pairs and euler angles MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Our low sampling rate means that the "cheap" hack for integrating quaternion rotations by using sin(x) ≃ x doesn't work, so instead we have to compute the partial rotation the hard way. Signed-off-by: Keith Packard --- src/core/ao_quaternion.h | 97 ++++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 94 insertions(+), 3 deletions(-) (limited to 'src/core/ao_quaternion.h') diff --git a/src/core/ao_quaternion.h b/src/core/ao_quaternion.h index 6a701a18..6c885500 100644 --- a/src/core/ao_quaternion.h +++ b/src/core/ao_quaternion.h @@ -110,17 +110,68 @@ static inline void ao_quaternion_normalize(struct ao_quaternion *r, } static inline void ao_quaternion_rotate(struct ao_quaternion *r, - struct ao_quaternion *a, - struct ao_quaternion *b) + const struct ao_quaternion *a, + const 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_conjugate(&c, b); ao_quaternion_multiply(r, &t, &c); } +/* + * Compute a rotation quaternion between two vectors + * + * cos(θ) + u * sin(θ) + * + * where θ is the angle between the two vectors and u + * is a unit vector axis of rotation + */ + +static inline void ao_quaternion_vectors_to_rotation(struct ao_quaternion *r, + const struct ao_quaternion *a, + const struct ao_quaternion *b) +{ + /* + * The cross product will point orthogonally to the two + * vectors, forming our rotation axis. The length will be + * sin(θ), so these values are already multiplied by that. + */ + + float x = a->y * b->z - a->z * b->y; + float y = a->z * b->x - a->x * b->z; + float z = a->x * b->y - a->y * b->x; + + float s_2 = x*x + y*y + z*z; + float s = sqrtf(s_2); + + /* cos(θ) = a · b / (|a| |b|). + * + * a and b are both unit vectors, so the divisor is one + */ + float c = a->x*b->x + a->y*b->y + a->z*b->z; + + float c_half = sqrtf ((1 + c) / 2); + float s_half = sqrtf ((1 - c) / 2); + + /* + * Divide out the sine factor from the + * cross product, then multiply in the + * half sine factor needed for the quaternion + */ + float s_scale = s_half / s; + + r->x = x * s_scale; + r->y = y * s_scale; + r->z = z * s_scale; + + r->r = c_half; + + ao_quaternion_normalize(r, r); +} + static inline void ao_quaternion_init_vector(struct ao_quaternion *r, float x, float y, float z) { @@ -146,4 +197,44 @@ static inline void ao_quaternion_init_zero_rotation(struct ao_quaternion *r) r->x = r->y = r->z = 0; } +/* + * The sincosf from newlib just calls sinf and cosf. This is a bit + * faster, if slightly less precise + */ + +static inline void +ao_sincosf(float a, float *s, float *c) { + float _s = sinf(a); + *s = _s; + *c = sqrtf(1 - _s*_s); +} + +/* + * Initialize a quaternion from 1/2 euler rotation angles (in radians). + * + * Yes, it would be nicer if there were a faster way, but because we + * sample the gyros at only 100Hz, we end up getting angles too large + * to take advantage of sin(x) ≃ x. + * + * We might be able to use just a couple of elements of the sin taylor + * series though, instead of the whole sin function? + */ + +static inline void ao_quaternion_init_half_euler(struct ao_quaternion *r, + float x, float y, float z) +{ + float s_x, c_x; + float s_y, c_y; + float s_z, c_z; + + ao_sincosf(x, &s_x, &c_x); + ao_sincosf(y, &s_y, &c_y); + ao_sincosf(z, &s_z, &c_z); + + r->r = c_x * c_y * c_z + s_x * s_y * s_z; + r->x = s_x * c_y * c_z - c_x * s_y * s_z; + r->y = c_x * s_y * c_z + s_x * c_y * s_z; + r->z = c_x * c_y * s_z - s_x * s_y * c_z; +} + #endif /* _AO_QUATERNION_H_ */ -- cgit v1.2.3 From bdd6244d8b4a55c9aa4fb79b0cb1a0727afbc2ac Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 12 Nov 2013 14:01:55 +0900 Subject: altos: Add orientation tracking to ao_flight_test Shows calculated offset from vertical in ao_flight_test output Signed-off-by: Keith Packard --- src/core/ao_data.h | 12 +++++ src/core/ao_quaternion.h | 9 ++++ src/test/ao_flight_test.c | 111 +++++++++++++++++++++++++++++++++++++++++++--- 3 files changed, 127 insertions(+), 5 deletions(-) (limited to 'src/core/ao_quaternion.h') diff --git a/src/core/ao_data.h b/src/core/ao_data.h index 5a232885..e1d8a139 100644 --- a/src/core/ao_data.h +++ b/src/core/ao_data.h @@ -319,4 +319,16 @@ typedef int16_t angle_t; /* in degrees */ #endif +#if !HAS_MAG && HAS_HMC5883 + +#define HAS_MAG 1 + +typedef int16_t ao_mag_t; /* in raw sample units */ + +#define ao_data_mag_along(packet) ((packet)->hmc5883.x) +#define ao_data_mag_across(packet) ((packet)->hmc5883.y) +#define ao_data_mag_through(packet) ((packet)->hmc5883.z) + +#endif + #endif /* _AO_DATA_H_ */ diff --git a/src/core/ao_quaternion.h b/src/core/ao_quaternion.h index 6c885500..044f1607 100644 --- a/src/core/ao_quaternion.h +++ b/src/core/ao_quaternion.h @@ -109,6 +109,15 @@ static inline void ao_quaternion_normalize(struct ao_quaternion *r, *r = *a; } +static inline float ao_quaternion_dot(const struct ao_quaternion *a, + const struct ao_quaternion *b) +{ +#define T(_a) (((a)->_a) * ((b)->_a)) + return T(r) + T(x) + T(y) + T(z); +#undef T +} + + static inline void ao_quaternion_rotate(struct ao_quaternion *r, const struct ao_quaternion *a, const struct ao_quaternion *b) diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 952a811a..452f5b75 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -48,6 +48,7 @@ int ao_gps_new; #define HAS_MS5607 1 #define HAS_MPU6000 1 #define HAS_MMA655X 1 +#define HAS_HMC5883 1 struct ao_adc { int16_t sense[AO_ADC_NUM_SENSE]; @@ -349,6 +350,23 @@ ao_test_exit(void) exit(0); } +#ifdef TELEMEGA +struct ao_azel { + int az; + int el; +}; + +static void +azel (struct ao_azel *r, struct ao_quaternion *q) +{ + double v; + + r->az = floor (atan2(q->y, q->x) * 180/M_PI + 0.5); + v = sqrt (q->x*q->x + q->y*q->y); + r->el = floor (atan2(q->z, v) * 180/M_PI + 0.5); +} +#endif + void ao_insert(void) { @@ -402,10 +420,86 @@ ao_insert(void) } if (!ao_summary) { +#if TELEMEGA + static struct ao_quaternion ao_ground_mag; + static int ao_ground_mag_set; + + if (!ao_ground_mag_set) { + ao_quaternion_init_vector (&ao_ground_mag, + ao_data_mag_across(&ao_data_static), + ao_data_mag_through(&ao_data_static), + ao_data_mag_along(&ao_data_static)); + ao_quaternion_normalize(&ao_ground_mag, &ao_ground_mag); + ao_quaternion_rotate(&ao_ground_mag, &ao_ground_mag, &ao_rotation); + ao_ground_mag_set = 1; + } + + struct ao_quaternion ao_mag, ao_mag_rot; + + ao_quaternion_init_vector(&ao_mag, + ao_data_mag_across(&ao_data_static), + ao_data_mag_through(&ao_data_static), + ao_data_mag_along(&ao_data_static)); + + ao_quaternion_normalize(&ao_mag, &ao_mag); + ao_quaternion_rotate(&ao_mag_rot, &ao_mag, &ao_rotation); + + float ao_dot; + int ao_mag_angle; + + ao_dot = ao_quaternion_dot(&ao_mag_rot, &ao_ground_mag); + + struct ao_azel ground_azel, mag_azel, rot_azel; + + azel(&ground_azel, &ao_ground_mag); + azel(&mag_azel, &ao_mag); + azel(&rot_azel, &ao_mag_rot); + + ao_mag_angle = floor (acos(ao_dot) * 180 / M_PI + 0.5); + + static struct ao_quaternion ao_x = { .r = 0, .x = 1, .y = 0, .z = 0 }; + struct ao_quaternion ao_out; + + ao_quaternion_rotate(&ao_out, &ao_x, &ao_rotation); + + int out = floor (atan2(ao_out.y, ao_out.x) * 180 / M_PI); + + printf ("%7.2f state %-8.8s height %8.4f tilt %4d rot %4d mag_tilt %4d mag_rot %4d\n", + time, + ao_state_names[ao_flight_state], + ao_k_height / 65536.0, + ao_sample_orient, out, + mag_azel.el, + mag_azel.az); + + +#if 0 + printf ("\t\tstate %-8.8s ground az: %4d el %4d mag az %4d el %4d rot az %4d el %4d el_diff %4d az_diff %4d angle %4d tilt %4d ground %8.5f %8.5f %8.5f cur %8.5f %8.5f %8.5f rot %8.5f %8.5f %8.5f\n", + ao_state_names[ao_flight_state], + ground_azel.az, ground_azel.el, + mag_azel.az, mag_azel.el, + rot_azel.az, rot_azel.el, + ground_azel.el - rot_azel.el, + ground_azel.az - rot_azel.az, + ao_mag_angle, + ao_sample_orient, + ao_ground_mag.x, + ao_ground_mag.y, + ao_ground_mag.z, + ao_mag.x, + ao_mag.y, + ao_mag.z, + ao_mag_rot.x, + ao_mag_rot.y, + ao_mag_rot.z); +#endif +#endif + +#if 0 printf("%7.2f height %8.2f accel %8.3f " #if TELEMEGA "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 " */ + "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f mag_x %8d mag_y %8d, mag_z %8d mag_angle %4d " #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, @@ -413,14 +507,17 @@ ao_insert(void) accel, #if TELEMEGA ao_sample_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), -*/ + ao_data_static.hmc5883.x, + ao_data_static.hmc5883.y, + ao_data_static.hmc5883.z, + ao_mag_angle, #endif ao_state_names[ao_flight_state], ao_k_height / 65536.0, @@ -430,6 +527,7 @@ ao_insert(void) drogue_height, main_height, ao_error_h_sq_avg); +#endif // if (ao_flight_state == ao_flight_landed) // ao_test_exit(); @@ -659,11 +757,14 @@ ao_sleep(void *wchan) ao_data_static.ms5607_raw.temp = int32(bytes, 4); ao_ms5607_convert(&ao_data_static.ms5607_raw, &value); ao_data_static.mpu6000.accel_x = int16(bytes, 8); - ao_data_static.mpu6000.accel_y = -int16(bytes, 10); + ao_data_static.mpu6000.accel_y = int16(bytes, 10); ao_data_static.mpu6000.accel_z = int16(bytes, 12); ao_data_static.mpu6000.gyro_x = int16(bytes, 14); - ao_data_static.mpu6000.gyro_y = -int16(bytes, 16); + ao_data_static.mpu6000.gyro_y = int16(bytes, 16); ao_data_static.mpu6000.gyro_z = int16(bytes, 18); + ao_data_static.hmc5883.x = int16(bytes, 20); + ao_data_static.hmc5883.y = int16(bytes, 22); + ao_data_static.hmc5883.z = int16(bytes, 24); #if HAS_MMA655X ao_data_static.mma655x = int16(bytes, 26); #endif -- cgit v1.2.3