altoslib: Clean up quaternion and rotation interfaces

Export euler to quaternion (instead of half_euler).
Provide angles to rotate rather than rates and time.
Add comments to quaternion and rotation code.

Signed-off-by: Keith Packard <keithp@keithp.com>

diff --git a/altoslib/AltosFlightSeries.java b/altoslib/AltosFlightSeries.java
index 45d3b4563207a0c5f4f11309c6e41064ca107392..57f1a49163f639879c6d2ce65f7b03d429aecab4 100644 (file)
--- 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 66b5f4f55d6a706030d812cdf1273abc927c452c..9217fc4159337c1af57a6a29f483b993fd468b2e 100644 (file)
--- 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 0de2451504fb82385c971a94dc656150385a0e43..305f932a0330f158fc363d0f140fe285b0facd6a 100644 (file)
--- 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 cfee819bc76610f81fd2d092a9b7c18c79126368..9ee3d57ddf814fe340ef53df61fa2e31758d5cae 100644 (file)
--- 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);
                        }
                }