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>
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;
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,
}
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 +
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
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);;
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;
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();
}
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);
}
}