diff options
| -rw-r--r-- | altoslib/AltosFlightSeries.java | 8 | ||||
| -rw-r--r-- | altoslib/AltosQuaternion.java | 43 | ||||
| -rw-r--r-- | altoslib/AltosRotation.java | 29 | ||||
| -rw-r--r-- | altoslib/AltosState.java | 10 |
4 files changed, 63 insertions, 27 deletions
diff --git a/altoslib/AltosFlightSeries.java b/altoslib/AltosFlightSeries.java index 45d3b456..57f1a491 100644 --- a/altoslib/AltosFlightSeries.java +++ b/altoslib/AltosFlightSeries.java @@ -363,11 +363,11 @@ public class AltosFlightSeries extends AltosDataListener { double dt = time - prev_time; if (dt > 0) { - double roll = AltosConvert.degrees_to_radians(roll_v.value); - double pitch = AltosConvert.degrees_to_radians(gyro_pitch.value(time)); - double yaw = AltosConvert.degrees_to_radians(gyro_yaw.value(time)); + double roll = AltosConvert.degrees_to_radians(roll_v.value) * dt; + double pitch = AltosConvert.degrees_to_radians(gyro_pitch.value(time)) * dt; + double yaw = AltosConvert.degrees_to_radians(gyro_yaw.value(time)) * dt; - rotation.rotate(dt, pitch, yaw, roll); + rotation.rotate(pitch, yaw, roll); orient_series.add(time, rotation.tilt()); } prev_time = time; diff --git a/altoslib/AltosQuaternion.java b/altoslib/AltosQuaternion.java index 66b5f4f5..9217fc41 100644 --- a/altoslib/AltosQuaternion.java +++ b/altoslib/AltosQuaternion.java @@ -22,6 +22,7 @@ public class AltosQuaternion { double r; /* real bit */ double x, y, z; /* imaginary bits */ + /* Multiply by b */ public AltosQuaternion multiply(AltosQuaternion b) { return new AltosQuaternion( this.r * b.r - this.x * b.x - this.y * b.y - this.z * b.z, @@ -31,35 +32,36 @@ public class AltosQuaternion { } public AltosQuaternion conjugate() { - return new AltosQuaternion( - this.r, - -this.x, - -this.y, - -this.z); + return new AltosQuaternion(this.r, + -this.x, + -this.y, + -this.z); } public double normal() { - return (this.r * this.r + - this.x * this.x + - this.y * this.y + - this.z * this.z); + return Math.sqrt(this.r * this.r + + this.x * this.x + + this.y * this.y + + this.z * this.z); } + /* Scale by a real value */ public AltosQuaternion scale(double b) { - return new AltosQuaternion( - this.r * b, - this.x * b, - this.y * b, - this.z * b); + return new AltosQuaternion(this.r * b, + this.x * b, + this.y * b, + this.z * b); } + /* Divide by the length to end up with a quaternion of length 1 */ public AltosQuaternion normalize() { double n = normal(); if (n <= 0) return this; - return scale(1/Math.sqrt(n)); + return scale(1/n); } + /* dot product */ public double dot(AltosQuaternion b) { return (this.r * b.r + this.x * b.x + @@ -67,10 +69,14 @@ public class AltosQuaternion { this.z * b.z); } + /* Rotate 'this' by 'b' */ public AltosQuaternion rotate(AltosQuaternion b) { return (b.multiply(this)).multiply(b.conjugate()); } + /* Given two vectors (this and b), compute a quaternion + * representing the rotation between them + */ public AltosQuaternion vectors_to_rotation(AltosQuaternion b) { /* * The cross product will point orthogonally to the two @@ -145,7 +151,12 @@ public class AltosQuaternion { return new AltosQuaternion(1, 0, 0, 0); } - static public AltosQuaternion half_euler(double x, double y, double z) { + static public AltosQuaternion euler(double x, double y, double z) { + + x = x / 2.0; + y = y / 2.0; + z = z / 2.0; + double s_x = Math.sin(x), c_x = Math.cos(x); double s_y = Math.sin(y), c_y = Math.cos(y); double s_z = Math.sin(z), c_z = Math.cos(z);; diff --git a/altoslib/AltosRotation.java b/altoslib/AltosRotation.java index 0de24515..305f932a 100644 --- a/altoslib/AltosRotation.java +++ b/altoslib/AltosRotation.java @@ -21,6 +21,28 @@ package org.altusmetrum.altoslib_12; public class AltosRotation extends AltosQuaternion { private AltosQuaternion 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 + * angle from vertical. + * + * rot = ao_rotation * vertical * ao_rotation° + * rot = ao_rotation * (0,0,0,1) * ao_rotation° + * = ((a.z, a.y, -a.x, a.r) * (a.r, -a.x, -a.y, -a.z)) .z + * + * = (-a.z * -a.z) + (a.y * -a.y) - (-a.x * -a.x) + (a.r * a.r) + * = a.z² - a.y² - a.x² + a.r² + * + * rot = ao_rotation * (0, 0, 0, -1) * ao_rotation° + * = ((-a.z, -a.y, a.x, -a.r) * (a.r, -a.x, -a.y, -a.z)) .z + * + * = (a.z * -a.z) + (-a.y * -a.y) - (a.x * -a.x) + (-a.r * a.r) + * = -a.z² + a.y² + a.x² - a.r² + * + * tilt = acos(rot); /* in radians */ + */ + public double tilt() { double rotz = rotation.z * rotation.z - rotation.y * rotation.y - rotation.x * rotation.x + rotation.r * rotation.r; @@ -28,8 +50,11 @@ public class AltosRotation extends AltosQuaternion { return tilt; } - public void rotate(double dt, double x, double y, double z) { - AltosQuaternion rot = AltosQuaternion.half_euler(x * dt / 2.0, y * dt / 2.0, z * dt / 2.0); + /* Given euler rotations in three axes, perform a combined rotation using + * quaternions + */ + public void rotate(double x, double y, double z) { + AltosQuaternion rot = AltosQuaternion.euler(x, y, z); rotation = rot.multiply(rotation).normalize(); } diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index cfee819b..9ee3d57d 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -931,12 +931,12 @@ public class AltosState extends AltosDataListener { if (last_imu_time != AltosLib.MISSING) { double t = time - last_imu_time; - double pitch = AltosConvert.degrees_to_radians(gyro_pitch); - double yaw = AltosConvert.degrees_to_radians(gyro_yaw); - double roll = AltosConvert.degrees_to_radians(gyro_roll); + if (t > 0 && gyro_pitch != AltosLib.MISSING && rotation != null) { + double pitch = AltosConvert.degrees_to_radians(gyro_pitch) * t; + double yaw = AltosConvert.degrees_to_radians(gyro_yaw) * t; + double roll = AltosConvert.degrees_to_radians(gyro_roll) * t; - if (t > 0 && pitch != AltosLib.MISSING && rotation != null) { - rotation.rotate(t, pitch, yaw, roll); + rotation.rotate(pitch, yaw, roll); orient.set_computed(rotation.tilt(), time); } } |