altoslib: Compute tilt angle from eeprom data
authorKeith Packard <keithp@keithp.com>
Sat, 4 Oct 2014 07:11:13 +0000 (00:11 -0700)
committerKeith Packard <keithp@keithp.com>
Sat, 4 Oct 2014 07:11:13 +0000 (00:11 -0700)
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 <keithp@keithp.com>
14 files changed:
altoslib/AltosCSV.java
altoslib/AltosConfigData.java
altoslib/AltosEepromHeader.java
altoslib/AltosEepromMega.java
altoslib/AltosIMU.java
altoslib/AltosLib.java
altoslib/AltosMag.java
altoslib/AltosQuaternion.java [new file with mode: 0644]
altoslib/AltosRotation.java [new file with mode: 0644]
altoslib/AltosState.java
altoslib/AltosTelemetryMegaSensor.java
altoslib/Makefile.am
altosuilib/AltosGraph.java
altosuilib/AltosGraphDataPoint.java

index 4a9278d901fefd1348120d4a5d72d4f75991cf8f..2357dbc77707d6e4bdfb302a40dfdb9f24bc5ddc 100644 (file)
@@ -162,17 +162,10 @@ public class AltosCSV implements AltosWriter {
        }
 
        void write_advanced(AltosState state) {
-               AltosIMU        imu = state.imu;
-               AltosMag        mag = state.mag;
-
-               if (imu == null)
-                       imu = new AltosIMU();
-               if (mag == null)
-                       mag = new AltosMag();
                out.printf("%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7.2f",
-                          imu.accel_x, imu.accel_y, imu.accel_z,
-                          imu.gyro_x, imu.gyro_y, imu.gyro_z,
-                          mag.x, mag.y, mag.z);
+                          state.accel_along(), state.accel_across(), state.accel_through(),
+                          state.gyro_roll(), state.gyro_pitch(), state.gyro_yaw(),
+                          state.mag_along(), state.mag_across(), state.mag_through());
        }
 
        void write_gps_header() {
@@ -381,7 +374,7 @@ public class AltosCSV implements AltosWriter {
                                has_basic = true;
                        if (state.battery_voltage != AltosLib.MISSING)
                                has_battery = true;
-                       if (state.imu != null || state.mag != null)
+                       if (state.accel_across() != AltosLib.MISSING)
                                has_advanced = true;
                        if (state.gps != null) {
                                has_gps = true;
index fc1f2442ebfa04fd66074a096449eda21022fdc3..a9e863c7759edac882571fa00ab7904d190b7602 100644 (file)
@@ -90,6 +90,9 @@ public class AltosConfigData implements Iterable<String> {
        public int      tracker_motion;
        public int      tracker_interval;
 
+       /* HAS_GYRO */
+       public int      accel_zero_along, accel_zero_across, accel_zero_through;
+
        public static String get_string(String line, String label) throws  ParseException {
                if (line.startsWith(label)) {
                        String  quoted = line.substring(label.length()).trim();
@@ -266,6 +269,10 @@ public class AltosConfigData implements Iterable<String> {
                storage_size = -1;
                storage_erase_unit = -1;
                stored_flight = 0;
+
+               accel_zero_along = -1;
+               accel_zero_across = -1;
+               accel_zero_through = -1;
        }
 
        public void parse_line(String line) {
@@ -361,6 +368,18 @@ public class AltosConfigData implements Iterable<String> {
 
                /* Log listing replies */
                try { get_int(line, "flight"); stored_flight++; }  catch (Exception e) {}
+
+               /* HAS_GYRO */
+               try {
+                       if (line.startsWith("IMU call along")) {
+                               String[] bits = line.split("\\s+");
+                               if (bits.length >= 8) {
+                                       accel_zero_along = Integer.parseInt(bits[3]);
+                                       accel_zero_across = Integer.parseInt(bits[5]);
+                                       accel_zero_through = Integer.parseInt(bits[7]);
+                               }
+                       }
+               } catch (Exception e) {}
        }
 
        public AltosConfigData() {
@@ -637,6 +656,9 @@ public class AltosConfigData implements Iterable<String> {
                if (tracker_motion >= 0 && tracker_interval >= 0)
                        link.printf("c t %d %d\n", tracker_motion, tracker_interval);
 
+               /* HAS_GYRO */
+               /* UI doesn't support accel cal */
+
                link.printf("c w\n");
                link.flush_output();
        }
index 7103065526aa693283f38587ee474cae75791c61..0414c37e39fc30cc4ea5dc5f54a4b0900058aa1f 100644 (file)
@@ -25,7 +25,7 @@ public class AltosEepromHeader extends AltosEeprom {
 
        public int      cmd;
        public String   data;
-       public int      config_a, config_b;
+       public int      config_a, config_b, config_c;
        public boolean  last;
        public boolean  valid;
 
@@ -93,6 +93,9 @@ public class AltosEepromHeader extends AltosEeprom {
                        state.make_baro();
                        state.baro.crc = config_a;
                        break;
+               case AltosLib.AO_LOG_IMU_CAL:
+                       state.set_accel_zero(config_a, config_b, config_c);
+                       break;
                case AltosLib.AO_LOG_SOFTWARE_VERSION:
                        state.set_firmware_version(data);
                        break;
@@ -100,7 +103,10 @@ public class AltosEepromHeader extends AltosEeprom {
                case AltosLib.AO_LOG_APOGEE_LOCKOUT:
                case AltosLib.AO_LOG_RADIO_RATE:
                case AltosLib.AO_LOG_IGNITE_MODE:
+                       break;
                case AltosLib.AO_LOG_PAD_ORIENTATION:
+                       state.set_pad_orientation(config_a);
+                       break;
                case AltosLib.AO_LOG_RADIO_ENABLE:
                case AltosLib.AO_LOG_AES_KEY:
                case AltosLib.AO_LOG_APRS:
@@ -177,11 +183,17 @@ public class AltosEepromHeader extends AltosEeprom {
                case AltosLib.AO_LOG_BARO_CRC:
                        out.printf ("# Baro crc: %d\n", config_a);
                        break;
+               case AltosLib.AO_LOG_IMU_CAL:
+                       out.printf ("# IMU cal: %d %d %d\n", config_a, config_b, config_c);
+                       break;
                case AltosLib.AO_LOG_FREQUENCY:
                case AltosLib.AO_LOG_APOGEE_LOCKOUT:
                case AltosLib.AO_LOG_RADIO_RATE:
                case AltosLib.AO_LOG_IGNITE_MODE:
+                       break;
                case AltosLib.AO_LOG_PAD_ORIENTATION:
+                       out.printf("# Pad orientation: %d\n", config_a);
+                       break;
                case AltosLib.AO_LOG_RADIO_ENABLE:
                case AltosLib.AO_LOG_AES_KEY:
                case AltosLib.AO_LOG_APRS:
@@ -273,6 +285,14 @@ public class AltosEepromHeader extends AltosEeprom {
                                        cmd = AltosLib.AO_LOG_INVALID;
                                        data = tokens[2];
                                }
+                       } else if (tokens[0].equals("IMU") && tokens[1].equals("cal")) {
+                               cmd = AltosLib.AO_LOG_IMU_CAL;
+                               config_a = Integer.parseInt(tokens[3]);
+                               config_b = Integer.parseInt(tokens[5]);
+                               config_c = Integer.parseInt(tokens[7]);
+                       } else if (tokens[0].equals("Pad") && tokens[1].equals("orientation:")) {
+                               cmd = AltosLib.AO_LOG_PAD_ORIENTATION;
+                               config_a = Integer.parseInt(tokens[2]);
                        } else
                                valid = false;
                } catch (Exception e) {
index aaff2410ff527588686c723e1efdac26007e7566..f9bfb4a74f335267be931f1ac6c663866552ec9b 100644 (file)
@@ -149,6 +149,12 @@ public class AltosEepromMega extends AltosEeprom {
                        state.set_flight(flight());
                        state.set_ground_accel(ground_accel());
                        state.set_ground_pressure(ground_pres());
+                       state.set_accel_ground(ground_accel_along(),
+                                              ground_accel_across(),
+                                              ground_accel_through());
+                       state.set_gyro_zero(ground_roll() / 512.0,
+                                           ground_pitch() / 512.0,
+                                           ground_yaw() / 512.0);
                        break;
                case AltosLib.AO_LOG_STATE:
                        state.set_tick(tick);
@@ -158,22 +164,21 @@ public class AltosEepromMega extends AltosEeprom {
                        state.set_tick(tick);
                        state.set_ms5607(pres(), temp());
 
-                       AltosIMU imu = new AltosIMU();
-                       imu.accel_x = AltosIMU.convert_accel(accel_x());
-                       imu.accel_y = AltosIMU.convert_accel(accel_y());
-                       imu.accel_z = AltosIMU.convert_accel(accel_z());
+                       AltosIMU        imu = new AltosIMU(accel_y(),   /* along */
+                                                          accel_x(),   /* across */
+                                                          accel_z(),   /* through */
+                                                          gyro_y(),    /* roll */
+                                                          gyro_x(),    /* pitch */
+                                                          gyro_z());   /* yaw */
 
-                       imu.gyro_x = AltosIMU.convert_gyro(gyro_x());
-                       imu.gyro_y = AltosIMU.convert_gyro(gyro_y());
-                       imu.gyro_z = AltosIMU.convert_gyro(gyro_z());
-                       state.imu = imu;
+                       if (log_format == AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD)
+                               state.check_imu_wrap(imu);
 
-                       AltosMag mag = new AltosMag();
-                       mag.x = AltosMag.convert_gauss(mag_x());
-                       mag.y = AltosMag.convert_gauss(mag_y());
-                       mag.z = AltosMag.convert_gauss(mag_z());
+                       state.set_imu(imu);
 
-                       state.mag = mag;
+                       state.set_mag(new AltosMag(mag_x(),
+                                                  mag_y(),
+                                                  mag_z()));
 
                        state.set_accel(accel());
 
index 89d7def4a3bb2066b5ff1adbdd364107dd575ad3..d7373f3c57ac7c47d88da8803e58a8f2e37ad89c 100644 (file)
@@ -20,36 +20,24 @@ package org.altusmetrum.altoslib_5;
 import java.util.concurrent.*;
 
 public class AltosIMU implements Cloneable {
-       public double           accel_x;
-       public double           accel_y;
-       public double           accel_z;
+       public int              accel_along;
+       public int              accel_across;
+       public int              accel_through;
 
-       public double           gyro_x;
-       public double           gyro_y;
-       public double           gyro_z;
+       public int              gyro_roll;
+       public int              gyro_pitch;
+       public int              gyro_yaw;
 
-/*
- * XXX use ground measurements to adjust values
-
-       public double           ground_accel_x;
-       public double           ground_accel_y;
-       public double           ground_accel_z;
-
-       public double           ground_gyro_x;
-       public double           ground_gyro_y;
-       public double           ground_gyro_z;
-*/
-
-       public static int       counts_per_g = 2048;
+       public static double    counts_per_g = 2048.0;
 
-       public static double convert_accel(int counts) {
-               return (double) counts / (double) counts_per_g * (-AltosConvert.GRAVITATIONAL_ACCELERATION);
+       public static double convert_accel(double counts) {
+               return counts / counts_per_g * (-AltosConvert.GRAVITATIONAL_ACCELERATION);
        }
 
        public static double    counts_per_degsec = 16.4;
 
-       public static double convert_gyro(int counts) {
-               return (double) counts / counts_per_degsec;
+       public static double convert_gyro(double counts) {
+               return counts / counts_per_degsec;
        }
 
        public boolean parse_string(String line) {
@@ -59,12 +47,12 @@ public class AltosIMU implements Cloneable {
                String[] items = line.split("\\s+");
 
                if (items.length >= 8) {
-                       accel_x = convert_accel(Integer.parseInt(items[1]));
-                       accel_y = convert_accel(Integer.parseInt(items[2]));
-                       accel_z = convert_accel(Integer.parseInt(items[3]));
-                       gyro_x = convert_gyro(Integer.parseInt(items[5]));
-                       gyro_y = convert_gyro(Integer.parseInt(items[6]));
-                       gyro_z = convert_gyro(Integer.parseInt(items[7]));
+                       accel_along = Integer.parseInt(items[1]);
+                       accel_across = Integer.parseInt(items[2]);
+                       accel_through = Integer.parseInt(items[3]);
+                       gyro_roll = Integer.parseInt(items[5]);
+                       gyro_pitch = Integer.parseInt(items[6]);
+                       gyro_yaw = Integer.parseInt(items[7]);
                }
                return true;
        }
@@ -72,13 +60,13 @@ public class AltosIMU implements Cloneable {
        public AltosIMU clone() {
                AltosIMU        n = new AltosIMU();
 
-               n.accel_x = accel_x;
-               n.accel_y = accel_y;
-               n.accel_z = accel_z;
+               n.accel_along = accel_along;
+               n.accel_across = accel_across;
+               n.accel_through = accel_through;
 
-               n.gyro_x = gyro_x;
-               n.gyro_y = gyro_y;
-               n.gyro_z = gyro_z;
+               n.gyro_roll = gyro_roll;
+               n.gyro_pitch = gyro_pitch;
+               n.gyro_yaw = gyro_yaw;
                return n;
        }
 
@@ -93,13 +81,25 @@ public class AltosIMU implements Cloneable {
        }
 
        public AltosIMU() {
-               accel_x = AltosLib.MISSING;
-               accel_y = AltosLib.MISSING;
-               accel_z = AltosLib.MISSING;
+               accel_along = AltosLib.MISSING;
+               accel_across = AltosLib.MISSING;
+               accel_through = AltosLib.MISSING;
+
+               gyro_roll = AltosLib.MISSING;
+               gyro_pitch = AltosLib.MISSING;
+               gyro_yaw = AltosLib.MISSING;
+       }
+
+       public AltosIMU(int accel_along, int accel_across, int accel_through,
+                       int gyro_roll, int gyro_pitch, int gyro_yaw) {
+
+               this.accel_along = accel_along;
+               this.accel_across = accel_across;
+               this.accel_through = accel_through;
 
-               gyro_x = AltosLib.MISSING;
-               gyro_y = AltosLib.MISSING;
-               gyro_z = AltosLib.MISSING;
+               this.gyro_roll = gyro_roll;
+               this.gyro_pitch = gyro_pitch;
+               this.gyro_yaw = gyro_yaw;
        }
 
        public AltosIMU(AltosLink link) throws InterruptedException, TimeoutException {
index 92a22ec77e8a4944856e90313336dac743ad9c1f..0edc0b43537588c7a17c6c7a63b4a84f98385f66 100644 (file)
@@ -74,6 +74,7 @@ public class AltosLib {
        public static final int AO_LOG_BARO_TREF = 3006;
        public static final int AO_LOG_BARO_TEMPSENS = 3007;
        public static final int AO_LOG_BARO_CRC = 3008;
+       public static final int AO_LOG_IMU_CAL = 3009;
 
        public static final int AO_LOG_SOFTWARE_VERSION = 9999;
 
index 690241f13e53c98ed97da0a8a76a0ef2c51aaaf6..1fa8877b9c639b4d1b5bdd714be5687424dd70be 100644 (file)
@@ -20,19 +20,19 @@ package org.altusmetrum.altoslib_5;
 import java.util.concurrent.*;
 
 public class AltosMag implements Cloneable {
-       public double           x;
-       public double           y;
-       public double           z;
+       public int              along;
+       public int              across;
+       public int              through;
 
        public static double counts_per_gauss = 1090;
 
-       public static double convert_gauss(int counts) {
-               return (double) counts / counts_per_gauss;
+       public static double convert_gauss(double counts) {
+               return counts / counts_per_gauss;
        }
 
        public boolean parse_string(String line) {
 //             if (line.startsWith("Syntax error")) {
-//                     x = y = z = 0;
+//                     along = across = through = 0;
 //                     return true;
 //             }
 
@@ -42,9 +42,9 @@ public class AltosMag implements Cloneable {
                String[] items = line.split("\\s+");
 
                if (items.length >= 6) {
-                       x = convert_gauss(Integer.parseInt(items[1]));
-                       y = convert_gauss(Integer.parseInt(items[3]));
-                       z = convert_gauss(Integer.parseInt(items[5]));
+                       along = Integer.parseInt(items[1]);
+                       across = Integer.parseInt(items[3]);
+                       through = Integer.parseInt(items[5]);
                }
                return true;
        }
@@ -52,16 +52,22 @@ public class AltosMag implements Cloneable {
        public AltosMag clone() {
                AltosMag n = new AltosMag();
 
-               n.x = x;
-               n.y = y;
-               n.z = z;
+               n.along = along;
+               n.across = across;
+               n.through = through;
                return n;
        }
 
        public AltosMag() {
-               x = AltosLib.MISSING;
-               y = AltosLib.MISSING;
-               z = AltosLib.MISSING;
+               along = AltosLib.MISSING;
+               across = AltosLib.MISSING;
+               through = AltosLib.MISSING;
+       }
+
+       public AltosMag(int along, int across, int through) {
+               this.along = along;
+               this.across = across;
+               this.through = through;
        }
 
        static public void update_state(AltosState state, AltosLink link, AltosConfigData config_data) throws InterruptedException {
diff --git a/altoslib/AltosQuaternion.java b/altoslib/AltosQuaternion.java
new file mode 100644 (file)
index 0000000..a712510
--- /dev/null
@@ -0,0 +1,150 @@
+/*
+ * Copyright © 2014 Keith Packard <keithp@keithp.com>
+ *
+ * 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 AltosQuaternion {
+       double  r;              /* real bit */
+       double  x, y, z;        /* imaginary bits */
+
+       public AltosQuaternion multiply(AltosQuaternion b) {
+               return new AltosQuaternion(
+                       this.r * b.r - this.x * b.x - this.y * b.y - this.z * b.z,
+                       this.r * b.x + this.x * b.r + this.y * b.z - this.z * b.y,
+                       this.r * b.y - this.x * b.z + this.y * b.r + this.z * b.x,
+                       this.r * b.z + this.x * b.y - this.y * b.x + this.z * b.r);
+       }
+
+       public AltosQuaternion conjugate() {
+               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);
+       }
+
+       public AltosQuaternion scale(double b) {
+               return new AltosQuaternion(
+                       this.r * b,
+                       this.x * b,
+                       this.y * b,
+                       this.z * b);
+       }
+
+       public AltosQuaternion normalize() {
+               double  n = normal();
+               if (n <= 0)
+                       return this;
+               return scale(1/Math.sqrt(n));
+       }
+
+       public double dot(AltosQuaternion b) {
+               return (this.r * b.r +
+                       this.x * b.x +
+                       this.y * b.y +
+                       this.z * b.z);
+       }
+
+       public AltosQuaternion rotate(AltosQuaternion b) {
+               return (b.multiply(this)).multiply(b.conjugate());
+       }
+
+       public AltosQuaternion vectors_to_rotation(AltosQuaternion 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.
+                */
+
+               double x = this.y * b.z - this.z * b.y;
+               double y = this.z * b.x - this.x * b.z;
+               double z = this.x * b.y - this.y * b.x;
+
+               double s_2 = x*x + y*y + z*z;
+               double s = Math.sqrt(s_2);
+
+               /* cos(θ) = a · b / (|a| |b|).
+                *
+                * a and b are both unit vectors, so the divisor is one
+                */
+               double c = this.x*b.x + this.y*b.y + this.z*b.z;
+
+               double c_half = Math.sqrt ((1 + c) / 2);
+               double s_half = Math.sqrt ((1 - c) / 2);
+
+               /*
+                * Divide out the sine factor from the
+                * cross product, then multiply in the
+                * half sine factor needed for the quaternion
+                */
+               double s_scale = s_half / s;
+
+               AltosQuaternion r = new AltosQuaternion(c_half,
+                                                       x * s_scale,
+                                                       y * s_scale,
+                                                       z * s_scale);
+               return r.normalize();
+       }
+
+       public AltosQuaternion(double r, double x, double y, double z) {
+               this.r = r;
+               this.x = x;
+               this.y = y;
+               this.z = z;
+       }
+
+       public AltosQuaternion(AltosQuaternion q) {
+               this.r = q.r;
+               this.x = q.x;
+               this.y = q.y;
+               this.z = q.z;
+       }
+
+       static public AltosQuaternion vector(double x, double y, double z) {
+               return new AltosQuaternion(0, x, y, z);
+       }
+
+       static public AltosQuaternion rotation(double x, double y, double z,
+                                              double s, double c) {
+               return new AltosQuaternion(c,
+                                          s*x,
+                                          s*y,
+                                          s*z);
+       }
+
+       static public AltosQuaternion zero_rotation() {
+               return new AltosQuaternion(1, 0, 0, 0);
+       }
+
+       static public AltosQuaternion half_euler(double x, double y, double z) {
+               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);;
+
+               return new AltosQuaternion(c_x * c_y * c_z + s_x * s_y * s_z,
+                                          s_x * c_y * c_z - c_x * s_y * s_z,
+                                          c_x * s_y * c_z + s_x * c_y * s_z,
+                                          c_x * c_y * s_z - s_x * s_y * c_z);
+       }
+}
diff --git a/altoslib/AltosRotation.java b/altoslib/AltosRotation.java
new file mode 100644 (file)
index 0000000..ad5aca8
--- /dev/null
@@ -0,0 +1,50 @@
+/*
+ * Copyright © 2014 Keith Packard <keithp@keithp.com>
+ *
+ * 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);
+       }
+}
index 5fce15c43865e63b43447db93f59faf725f838f7..830e95f311c19d6af0c0ef78821aa565bff55ef2 100644 (file)
@@ -44,8 +44,8 @@ public class AltosState implements Cloneable {
        public int      boost_tick;
 
        class AltosValue {
-               private double  value;
-               private double  prev_value;
+               double  value;
+               double  prev_value;
                private double  max_value;
                private double  set_time;
                private double  prev_set_time;
@@ -180,8 +180,19 @@ public class AltosState implements Cloneable {
        }
 
        class AltosCValue {
-               AltosValue      measured;
-               AltosValue      computed;
+
+               class AltosIValue extends AltosValue {
+                       boolean can_max() {
+                               return c_can_max();
+                       }
+               };
+
+               public AltosIValue      measured;
+               public AltosIValue      computed;
+
+               boolean can_max() { return true; }
+
+               boolean c_can_max() { return can_max(); }
 
                double value() {
                        double v = measured.value();
@@ -263,8 +274,8 @@ public class AltosState implements Cloneable {
                }
 
                AltosCValue() {
-                       measured = new AltosValue();
-                       computed = new AltosValue();
+                       measured = new AltosIValue();
+                       computed = new AltosIValue();
                }
        }
 
@@ -600,10 +611,10 @@ public class AltosState implements Cloneable {
                return acceleration.max();
        }
 
-       public AltosValue       orient;
+       public AltosCValue      orient;
 
        public void set_orient(double new_orient) {
-               orient.set(new_orient, time);
+               orient.set_measured(new_orient, time);
        }
 
        public double orient() {
@@ -713,7 +724,7 @@ public class AltosState implements Cloneable {
                pressure = new AltosPressure();
                speed = new AltosSpeed();
                acceleration = new AltosAccel();
-               orient = new AltosValue();
+               orient = new AltosCValue();
 
                temperature = AltosLib.MISSING;
                battery_voltage = AltosLib.MISSING;
@@ -733,7 +744,24 @@ public class AltosState implements Cloneable {
                gps_pending = false;
 
                imu = null;
+               last_imu_time = AltosLib.MISSING;
+               rotation = null;
+               ground_rotation = null;
+
                mag = null;
+               accel_zero_along = AltosLib.MISSING;
+               accel_zero_across = AltosLib.MISSING;
+               accel_zero_through = AltosLib.MISSING;
+
+               accel_ground_along = AltosLib.MISSING;
+               accel_ground_across = AltosLib.MISSING;
+               accel_ground_through = AltosLib.MISSING;
+
+               pad_orientation = AltosLib.MISSING;
+
+               gyro_zero_roll = AltosLib.MISSING;
+               gyro_zero_pitch = AltosLib.MISSING;
+               gyro_zero_yaw = AltosLib.MISSING;
 
                set_npad(0);
                ngps = 0;
@@ -861,6 +889,27 @@ public class AltosState implements Cloneable {
                        imu = old.imu.clone();
                else
                        imu = null;
+               last_imu_time = old.last_imu_time;
+
+               if (old.rotation != null)
+                       rotation = new AltosRotation (old.rotation);
+
+               if (old.ground_rotation != null) {
+                       ground_rotation = new AltosRotation(old.ground_rotation);
+               }
+
+               accel_zero_along = old.accel_zero_along;
+               accel_zero_across = old.accel_zero_across;
+               accel_zero_through = old.accel_zero_through;
+
+               accel_ground_along = old.accel_ground_along;
+               accel_ground_across = old.accel_ground_across;
+               accel_ground_through = old.accel_ground_through;
+               pad_orientation = old.pad_orientation;
+
+               gyro_zero_roll = old.gyro_zero_roll;
+               gyro_zero_pitch = old.gyro_zero_pitch;
+               gyro_zero_yaw = old.gyro_zero_yaw;
 
                if (old.mag != null)
                        mag = old.mag.clone();
@@ -1112,16 +1161,170 @@ public class AltosState implements Cloneable {
                }
        }
 
+
+       public double   accel_zero_along;
+       public double   accel_zero_across;
+       public double   accel_zero_through;
+
+       public AltosRotation    rotation;
+       public AltosRotation    ground_rotation;
+
+       public void set_accel_zero(double zero_along, double zero_across, double zero_through) {
+               if (zero_along != AltosLib.MISSING) {
+                       accel_zero_along = zero_along;
+                       accel_zero_across = zero_across;
+                       accel_zero_through = zero_through;
+               }
+       }
+
+       public int pad_orientation;
+
+       public double   accel_ground_along, accel_ground_across, accel_ground_through;
+
+       void update_pad_rotation() {
+               if (pad_orientation != AltosLib.MISSING && accel_ground_along != AltosLib.MISSING) {
+                       rotation = new AltosRotation(AltosIMU.convert_accel(accel_ground_across - accel_zero_across),
+                                                    AltosIMU.convert_accel(accel_ground_through - accel_zero_through),
+                                                    AltosIMU.convert_accel(accel_ground_along - accel_zero_along),
+                                                    pad_orientation);
+                       ground_rotation = rotation;
+                       orient.set_computed(rotation.tilt(), time);
+               }
+       }
+
+       public void set_accel_ground(double ground_along, double ground_across, double ground_through) {
+               accel_ground_along = ground_along;
+               accel_ground_across = ground_across;
+               accel_ground_through = ground_through;
+               update_pad_rotation();
+       }
+
+       public void set_pad_orientation(int pad_orientation) {
+               this.pad_orientation = pad_orientation;
+               update_pad_rotation();
+       }
+
+       public double   gyro_zero_roll;
+       public double   gyro_zero_pitch;
+       public double   gyro_zero_yaw;
+
+       public void set_gyro_zero(double roll, double pitch, double yaw) {
+               if (roll != AltosLib.MISSING) {
+                       gyro_zero_roll = roll;
+                       gyro_zero_pitch = pitch;
+                       gyro_zero_yaw = yaw;
+               }
+       }
+
+       public double   last_imu_time;
+
+       private double radians(double degrees) {
+               if (degrees == AltosLib.MISSING)
+                       return AltosLib.MISSING;
+               return degrees * Math.PI / 180.0;
+       }
+
+       private void update_orient() {
+               if (last_imu_time != AltosLib.MISSING) {
+                       double  t = time - last_imu_time;
+
+                       double  pitch = radians(gyro_pitch());
+                       double  yaw = radians(gyro_yaw());
+                       double  roll = radians(gyro_roll());
+
+                       if (t > 0 & pitch != AltosLib.MISSING && rotation != null) {
+                               rotation.rotate(t, pitch, yaw, roll);
+                               orient.set_computed(rotation.tilt(), time);
+                       }
+               }
+               last_imu_time = time;
+       }
+
        public void set_imu(AltosIMU imu) {
                if (imu != null)
                        imu = imu.clone();
                this.imu = imu;
+               update_orient();
+       }
+
+       private double gyro_zero_overflow(double first) {
+               double v = first / 128.0;
+               if (v < 0)
+                       v = Math.ceil(v);
+               else
+                       v = Math.floor(v);
+               return v * 128.0;
+       }
+
+       public void check_imu_wrap(AltosIMU imu) {
+               if (this.imu == null) {
+                       gyro_zero_roll += gyro_zero_overflow(imu.gyro_roll);
+                       gyro_zero_pitch += gyro_zero_overflow(imu.gyro_pitch);
+                       gyro_zero_yaw += gyro_zero_overflow(imu.gyro_yaw);
+               }
+       }
+
+       public double accel_along() {
+               if (imu != null && accel_zero_along != AltosLib.MISSING)
+                       return AltosIMU.convert_accel(imu.accel_along - accel_zero_along);
+               return AltosLib.MISSING;
+       }
+
+       public double accel_across() {
+               if (imu != null && accel_zero_across != AltosLib.MISSING)
+                       return AltosIMU.convert_accel(imu.accel_across - accel_zero_across);
+               return AltosLib.MISSING;
+       }
+
+       public double accel_through() {
+               if (imu != null && accel_zero_through != AltosLib.MISSING)
+                       return AltosIMU.convert_accel(imu.accel_through - accel_zero_through);
+               return AltosLib.MISSING;
+       }
+
+       public double gyro_roll() {
+               if (imu != null && gyro_zero_roll != AltosLib.MISSING) {
+                       return AltosIMU.convert_gyro(imu.gyro_roll - gyro_zero_roll);
+               }
+               return AltosLib.MISSING;
+       }
+
+       public double gyro_pitch() {
+               if (imu != null && gyro_zero_pitch != AltosLib.MISSING) {
+                       return AltosIMU.convert_gyro(imu.gyro_pitch - gyro_zero_pitch);
+               }
+               return AltosLib.MISSING;
+       }
+
+       public double gyro_yaw() {
+               if (imu != null && gyro_zero_yaw != AltosLib.MISSING) {
+                       return AltosIMU.convert_gyro(imu.gyro_yaw - gyro_zero_yaw);
+               }
+               return AltosLib.MISSING;
        }
 
        public void set_mag(AltosMag mag) {
                this.mag = mag.clone();
        }
 
+       public double mag_along() {
+               if (mag != null)
+                       return AltosMag.convert_gauss(mag.along);
+               return AltosLib.MISSING;
+       }
+
+       public double mag_across() {
+               if (mag != null)
+                       return AltosMag.convert_gauss(mag.across);
+               return AltosLib.MISSING;
+       }
+
+       public double mag_through() {
+               if (mag != null)
+                       return AltosMag.convert_gauss(mag.through);
+               return AltosLib.MISSING;
+       }
+
        public AltosMs5607 make_baro() {
                if (baro == null)
                        baro = new AltosMs5607();
index 1b568c88cea80b47ca45bc6edc5d63559b32514d..bae18d1c252a5712746fd0a2748348116b429a0c 100644 (file)
@@ -66,24 +66,13 @@ public class AltosTelemetryMegaSensor extends AltosTelemetryStandard {
 
                state.set_orient(orient);
 
-               AltosIMU imu = new AltosIMU();
-
-               imu.accel_x = AltosIMU.convert_accel(accel_x);
-               imu.accel_y = AltosIMU.convert_accel(accel_y);
-               imu.accel_z = AltosIMU.convert_accel(accel_z);
-
-               imu.gyro_x = AltosIMU.convert_gyro(gyro_x);
-               imu.gyro_y = AltosIMU.convert_gyro(gyro_y);
-               imu.gyro_z = AltosIMU.convert_gyro(gyro_z);
-
-               state.imu = imu;
-
-               AltosMag mag = new AltosMag();
-
-               mag.x = AltosMag.convert_gauss(mag_x);
-               mag.y = AltosMag.convert_gauss(mag_y);
-               mag.z = AltosMag.convert_gauss(mag_z);
-
-               state.mag = mag;
+               state.set_imu(new AltosIMU(accel_y,     /* along */
+                                          accel_x,     /* across */
+                                          accel_z,     /* through */
+                                          gyro_y,      /* along */
+                                          gyro_x,      /* across */
+                                          gyro_z));    /* through */
+
+               state.set_mag(new AltosMag(mag_x, mag_y, mag_z));
        }
 }
index 0ad9b7dad226236c8d8b1212bb9794c92fa60f65..2805742e3e4eb626f91321685d95649d0eddbd88 100644 (file)
@@ -124,7 +124,9 @@ altoslib_JAVA = \
        AltosLatitude.java \
        AltosLongitude.java \
        AltosPyro.java \
-       AltosWriter.java
+       AltosWriter.java \
+       AltosQuaternion.java \
+       AltosRotation.java
 
 JAR=altoslib_$(ALTOSLIB_VERSION).jar
 
index 522eea1ef65ed1d463a9d84a303e213ef65f1c96..2e6d428d744658210a5a4f1f2dcfdc330c348a1e 100644 (file)
@@ -410,58 +410,58 @@ public class AltosGraph extends AltosUIGraph {
                }
 
                if (stats.has_imu) {
-                       addSeries("Acceleration X",
-                                 AltosGraphDataPoint.data_accel_x,
+                       addSeries("Acceleration Along",
+                                 AltosGraphDataPoint.data_accel_along,
                                  AltosConvert.accel,
                                  accel_x_color,
                                  false,
                                  accel_axis);
-                       addSeries("Acceleration Y",
-                                 AltosGraphDataPoint.data_accel_y,
+                       addSeries("Acceleration Across",
+                                 AltosGraphDataPoint.data_accel_across,
                                  AltosConvert.accel,
                                  accel_y_color,
                                  false,
                                  accel_axis);
-                       addSeries("Acceleration Z",
-                                 AltosGraphDataPoint.data_accel_z,
+                       addSeries("Acceleration Through",
+                                 AltosGraphDataPoint.data_accel_through,
                                  AltosConvert.accel,
                                  accel_z_color,
                                  false,
                                  accel_axis);
-                       addSeries("Rotation Rate X",
-                                 AltosGraphDataPoint.data_gyro_x,
+                       addSeries("Roll Rate",
+                                 AltosGraphDataPoint.data_gyro_roll,
                                  gyro_units,
                                  gyro_x_color,
                                  false,
                                  gyro_axis);
-                       addSeries("Rotation Rate Y",
-                                 AltosGraphDataPoint.data_gyro_y,
+                       addSeries("Pitch Rate",
+                                 AltosGraphDataPoint.data_gyro_pitch,
                                  gyro_units,
                                  gyro_y_color,
                                  false,
                                  gyro_axis);
-                       addSeries("Rotation Rate Z",
-                                 AltosGraphDataPoint.data_gyro_z,
+                       addSeries("Yaw Rate",
+                                 AltosGraphDataPoint.data_gyro_yaw,
                                  gyro_units,
                                  gyro_z_color,
                                  false,
                                  gyro_axis);
                }
                if (stats.has_mag) {
-                       addSeries("Magnetometer X",
-                                 AltosGraphDataPoint.data_mag_x,
+                       addSeries("Magnetometer Along",
+                                 AltosGraphDataPoint.data_mag_along,
                                  mag_units,
                                  mag_x_color,
                                  false,
                                  mag_axis);
-                       addSeries("Magnetometer Y",
-                                 AltosGraphDataPoint.data_mag_y,
+                       addSeries("Magnetometer Across",
+                                 AltosGraphDataPoint.data_mag_across,
                                  mag_units,
                                  mag_y_color,
                                  false,
                                  mag_axis);
-                       addSeries("Magnetometer Z",
-                                 AltosGraphDataPoint.data_mag_z,
+                       addSeries("Magnetometer Through",
+                                 AltosGraphDataPoint.data_mag_through,
                                  mag_units,
                                  mag_z_color,
                                  false,
index 56dadb8b49170389975d6063c77ce4cccd2e3539..a9f4f431387c1f4d36c765a8babbdd2024507545 100644 (file)
@@ -40,15 +40,15 @@ public class AltosGraphDataPoint implements AltosUIDataPoint {
        public static final int data_range = 14;
        public static final int data_distance = 15;
        public static final int data_pressure = 16;
-       public static final int data_accel_x = 17;
-       public static final int data_accel_y = 18;
-       public static final int data_accel_z = 19;
-       public static final int data_gyro_x = 20;
-       public static final int data_gyro_y = 21;
-       public static final int data_gyro_z = 22;
-       public static final int data_mag_x = 23;
-       public static final int data_mag_y = 24;
-       public static final int data_mag_z = 25;
+       public static final int data_accel_along = 17;
+       public static final int data_accel_across = 18;
+       public static final int data_accel_through = 19;
+       public static final int data_gyro_roll = 20;
+       public static final int data_gyro_pitch = 21;
+       public static final int data_gyro_yaw = 22;
+       public static final int data_mag_along = 23;
+       public static final int data_mag_across = 24;
+       public static final int data_mag_through = 25;
        public static final int data_orient = 26;
        public static final int data_gps_course = 27;
        public static final int data_gps_ground_speed = 28;
@@ -128,53 +128,32 @@ public class AltosGraphDataPoint implements AltosUIDataPoint {
                        y = state.pressure();
                        break;
 
-               case data_accel_x:
-               case data_accel_y:
-               case data_accel_z:
-               case data_gyro_x:
-               case data_gyro_y:
-               case data_gyro_z:
-                       AltosIMU        imu = state.imu;
-                       if (imu == null)
-                               break;
-                       switch (index) {
-                       case data_accel_x:
-                               y = imu.accel_x;
-                               break;
-                       case data_accel_y:
-                               y = imu.accel_y;
-                               break;
-                       case data_accel_z:
-                               y = imu.accel_z;
-                               break;
-                       case data_gyro_x:
-                               y = imu.gyro_x;
-                               break;
-                       case data_gyro_y:
-                               y = imu.gyro_y;
-                               break;
-                       case data_gyro_z:
-                               y = imu.gyro_z;
-                               break;
-                       }
+               case data_accel_along:
+                       y = state.accel_along();
                        break;
-               case data_mag_x:
-               case data_mag_y:
-               case data_mag_z:
-                       AltosMag        mag = state.mag;
-                       if (mag == null)
-                               break;
-                       switch (index) {
-                       case data_mag_x:
-                               y = mag.x;
-                               break;
-                       case data_mag_y:
-                               y = mag.y;
-                               break;
-                       case data_mag_z:
-                               y = mag.z;
-                               break;
-                       }
+               case data_accel_across:
+                       y = state.accel_across();
+                       break;
+               case data_accel_through:
+                       y = state.accel_through();
+                       break;
+               case data_gyro_roll:
+                       y = state.gyro_roll();
+                       break;
+               case data_gyro_pitch:
+                       y = state.gyro_pitch();
+                       break;
+               case data_gyro_yaw:
+                       y = state.gyro_yaw();
+                       break;
+               case data_mag_along:
+                       y = state.mag_along();
+                       break;
+               case data_mag_across:
+                       y = state.mag_across();
+                       break;
+               case data_mag_through:
+                       y = state.mag_through();
                        break;
                case data_orient:
                        y = state.orient();