From 00ae706dab6e8fddef4c5730a17c433a022228b7 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 4 Oct 2014 00:11:13 -0700 Subject: altoslib: Compute tilt angle from eeprom data This copies the computation of tilt angle from the firmware so that post-flight analysis can also show the data. This change also renames all of the imu values to make them easier to understand: accel gyro axis along roll length of the board across pitch across the board through yaw through the board. Signed-off-by: Keith Packard --- altoslib/AltosRotation.java | 50 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 50 insertions(+) create mode 100644 altoslib/AltosRotation.java (limited to 'altoslib/AltosRotation.java') diff --git a/altoslib/AltosRotation.java b/altoslib/AltosRotation.java new file mode 100644 index 00000000..ad5aca8f --- /dev/null +++ b/altoslib/AltosRotation.java @@ -0,0 +1,50 @@ +/* + * Copyright © 2014 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. + */ + +package org.altusmetrum.altoslib_5; + +public class AltosRotation { + private AltosQuaternion rotation; + + public double tilt() { + double rotz = rotation.z * rotation.z - rotation.y * rotation.y - rotation.x * rotation.x + rotation.r * rotation.r; + + double tilt = Math.acos(rotz) * 180.0 / Math.PI; + return tilt; + } + + public void rotate(double dt, double x, double y, double z) { + AltosQuaternion rot = AltosQuaternion.half_euler(x * dt, y * dt, z * dt); + rotation = rot.multiply(rotation).normalize(); + } + + /* Clone an existing rotation value */ + public AltosRotation (AltosRotation old) { + this.rotation = new AltosQuaternion(old.rotation); + } + + /* Create a new rotation value given an acceleration vector pointing down */ + public AltosRotation(double x, + double y, + double z, + int pad_orientation) { + AltosQuaternion orient = AltosQuaternion.vector(x, y, z).normalize(); + double sky = pad_orientation == 0 ? 1 : -1; + AltosQuaternion up = new AltosQuaternion(0, 0, 0, sky); + rotation = up.vectors_to_rotation(orient); + } +} -- cgit v1.2.3