altoslib: Compute orientation from eeprom data files
authorKeith Packard <keithp@keithp.com>
Fri, 9 Jun 2017 03:37:58 +0000 (20:37 -0700)
committerKeith Packard <keithp@keithp.com>
Fri, 9 Jun 2017 03:37:58 +0000 (20:37 -0700)
This was lost in the AltosFlightSeries transformation.

Signed-off-by: Keith Packard <keithp@keithp.com>
altoslib/AltosCalData.java
altoslib/AltosConvert.java
altoslib/AltosEepromRecordMega.java
altoslib/AltosFlightSeries.java
altoslib/AltosIMU.java
altoslib/AltosState.java

index 2eff6ac..49b90ce 100644 (file)
@@ -313,25 +313,27 @@ public class AltosCalData {
                        gyro_zero_roll = roll;
                        gyro_zero_pitch = pitch;
                        gyro_zero_yaw = yaw;
+                       imu_wrap_checked = false;
                }
        }
 
        public double gyro_roll(double counts) {
                if (gyro_zero_roll == AltosLib.MISSING || counts == AltosLib.MISSING)
                        return AltosLib.MISSING;
-               return AltosIMU.convert_gyro(counts - gyro_zero_roll);
+
+               return AltosIMU.gyro_degrees_per_second(counts, gyro_zero_roll);
        }
 
        public double gyro_pitch(double counts) {
                if (gyro_zero_pitch == AltosLib.MISSING || counts == AltosLib.MISSING)
                        return AltosLib.MISSING;
-               return AltosIMU.convert_gyro(counts - gyro_zero_pitch);
+               return AltosIMU.gyro_degrees_per_second(counts, gyro_zero_pitch);
        }
 
        public double gyro_yaw(double counts) {
                if (gyro_zero_yaw == AltosLib.MISSING || counts == AltosLib.MISSING)
                        return AltosLib.MISSING;
-               return AltosIMU.convert_gyro(counts - gyro_zero_yaw);
+               return AltosIMU.gyro_degrees_per_second(counts, gyro_zero_yaw);
        }
 
        private double gyro_zero_overflow(double first) {
@@ -340,13 +342,25 @@ public class AltosCalData {
                        v = Math.ceil(v);
                else
                        v = Math.floor(v);
+               if (v != 0)
+                       System.out.printf("Adjusting gyro axis by %g steps\n", v);
                return v * 128.0;
        }
 
+       /* Initial TeleMega log format had only 16 bits for gyro cal, so the top 9 bits got lost as the
+        * cal data are scaled by 512. Use the first sample to adjust the cal value, assuming that it is
+        * from a time of fairly low rotation speed. Fixed in later TeleMega firmware by storing 32 bits
+        * of cal values.
+        */
+       private boolean imu_wrap_checked = false;
+
        public void check_imu_wrap(double roll, double pitch, double yaw) {
-               gyro_zero_roll += gyro_zero_overflow(roll);
-               gyro_zero_pitch += gyro_zero_overflow(pitch);
-               gyro_zero_yaw += gyro_zero_overflow(yaw);
+               if (!imu_wrap_checked) {
+                       gyro_zero_roll += gyro_zero_overflow(roll);
+                       gyro_zero_pitch += gyro_zero_overflow(pitch);
+                       gyro_zero_yaw += gyro_zero_overflow(yaw);
+                       imu_wrap_checked = true;
+               }
        }
 
        public double mag_along(double along) {
index a252abd..0d25c6d 100644 (file)
@@ -184,6 +184,18 @@ public class AltosConvert {
                return altitude;
        }
 
+       public static double degrees_to_radians(double degrees) {
+               if (degrees == AltosLib.MISSING)
+                       return AltosLib.MISSING;
+               return degrees * (Math.PI / 180.0);
+       }
+
+       public static double radians_to_degrees(double radians) {
+               if (radians == AltosLib.MISSING)
+                       return AltosLib.MISSING;
+               return radians * (180.0 / Math.PI);
+       }
+
        public static double
        cc_battery_to_voltage(double battery)
        {
@@ -392,6 +404,7 @@ public class AltosConvert {
        }
 
        public static double acceleration_from_sensor(double sensor, double plus_g, double minus_g, double ground) {
+
                if (sensor == AltosLib.MISSING)
                        return AltosLib.MISSING;
 
index 0abc3fe..18d435a 100644 (file)
@@ -161,11 +161,13 @@ public class AltosEepromRecordMega extends AltosEepromRecord {
                                         cal_data.mag_through(mag_through));
 
 
+                       final double lsb_per_g = 1920.0/105.5;
+
                        double acceleration = AltosConvert.acceleration_from_sensor(
                                accel(),
-                               config_data.accel_cal_plus,
-                               config_data.accel_cal_minus,
-                               AltosLib.MISSING);
+                               cal_data.ground_accel,
+                               cal_data.ground_accel + 2 * lsb_per_g,
+                               cal_data.ground_accel);
 
                        listener.set_acceleration(acceleration);
                        break;
index 0b60fdf..dad066d 100644 (file)
@@ -304,6 +304,49 @@ public class AltosFlightSeries extends AltosDataListener {
                        add_series(speed_series);
        }
 
+       public AltosTimeSeries orient_series;
+
+       public static final String orient_name = "Tilt Angle";
+
+       private void compute_orient() {
+
+               if (orient_series != null)
+                       return;
+
+               if (accel_ground_across == AltosLib.MISSING)
+                       return;
+
+               if (cal_data.pad_orientation == AltosLib.MISSING)
+                       return;
+
+               if (cal_data.accel_zero_across == AltosLib.MISSING)
+                       return;
+
+               AltosRotation rotation = new AltosRotation(AltosIMU.convert_accel(accel_ground_across - cal_data.accel_zero_across),
+                                                          AltosIMU.convert_accel(accel_ground_through - cal_data.accel_zero_through),
+                                                          AltosIMU.convert_accel(accel_ground_along - cal_data.accel_zero_along),
+                                                          cal_data.pad_orientation);
+               double prev_time = ground_time;
+
+               orient_series = add_series(orient_name, AltosConvert.orient);
+               orient_series.add(ground_time, rotation.tilt());
+
+               for (AltosTimeValue roll_v : gyro_roll) {
+                       double  time = roll_v.time;
+                       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));
+
+                               rotation.rotate(dt, pitch, yaw, roll);
+                               orient_series.add(time, rotation.tilt());
+                       }
+                       prev_time = time;
+               }
+       }
+
        public AltosTimeSeries  kalman_height_series, kalman_speed_series, kalman_accel_series;
 
        public static final String kalman_height_name = "Kalman Height";
@@ -499,7 +542,17 @@ public class AltosFlightSeries extends AltosDataListener {
                accel_through.add(time(), through);
        }
 
+       private double  accel_ground_along = AltosLib.MISSING;
+       private double  accel_ground_across = AltosLib.MISSING;
+       private double  accel_ground_through = AltosLib.MISSING;
+
+       private double          ground_time;
+
        public  void set_accel_ground(double along, double across, double through) {
+               accel_ground_along = along;
+               accel_ground_across = across;
+               accel_ground_through = through;
+               ground_time = time();
        }
 
        public  void set_gyro(double roll, double pitch, double yaw) {
@@ -524,10 +577,6 @@ public class AltosFlightSeries extends AltosDataListener {
                mag_through.add(time(), through);
        }
 
-       public static final String orient_name = "Tilt Angle";
-
-       public AltosTimeSeries orient_series;
-
        public void set_orient(double orient) {
                if (orient_series == null)
                        orient_series = add_series(orient_name, AltosConvert.orient);
@@ -604,6 +653,7 @@ public class AltosFlightSeries extends AltosDataListener {
        }
 
        public void finish() {
+               compute_orient();
                compute_speed();
                compute_accel();
                compute_height();
index 20a2a41..b434e02 100644 (file)
@@ -36,10 +36,12 @@ public class AltosIMU implements Cloneable {
                return counts / counts_per_g * AltosConvert.gravity;
        }
 
-       public static final double      counts_per_degsec = 16.4;
+       /* In radians */
+       public static final double      GYRO_FULLSCALE_DEGREES = 2000.0;
+       public static final double      GYRO_COUNTS = 32767.0;
 
-       public static double convert_gyro(double counts) {
-               return counts / counts_per_degsec;
+       public static double gyro_degrees_per_second(double counts, double cal) {
+               return (counts - cal) * GYRO_FULLSCALE_DEGREES / GYRO_COUNTS;
        }
 
        public boolean parse_string(String line) {
index 5abc0c2..caa1cb4 100644 (file)
@@ -22,8 +22,6 @@
 
 package org.altusmetrum.altoslib_11;
 
-import java.io.*;
-
 public class AltosState extends AltosDataListener {
 
        public static final int set_position = 1;
@@ -759,8 +757,6 @@ public class AltosState extends AltosDataListener {
                accel_ground_across = AltosLib.MISSING;
                accel_ground_through = AltosLib.MISSING;
 
-               pad_orientation = AltosLib.MISSING;
-
                set_npad(0);
                ngps = 0;
 
@@ -903,16 +899,14 @@ public class AltosState extends AltosDataListener {
        public AltosRotation    rotation;
        public AltosRotation    ground_rotation;
 
-       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) {
+               if (cal_data.pad_orientation != AltosLib.MISSING && accel_ground_along != AltosLib.MISSING) {
                        rotation = new AltosRotation(AltosIMU.convert_accel(accel_ground_across - cal_data.accel_zero_across),
                                                     AltosIMU.convert_accel(accel_ground_through - cal_data.accel_zero_through),
                                                     AltosIMU.convert_accel(accel_ground_along - cal_data.accel_zero_along),
-                                                    pad_orientation);
+                                                    cal_data.pad_orientation);
                        ground_rotation = rotation;
                        orient.set_computed(rotation.tilt(), time);
                }
@@ -925,28 +919,17 @@ public class AltosState extends AltosDataListener {
                update_pad_rotation();
        }
 
-       public void set_pad_orientation(int pad_orientation) {
-               this.pad_orientation = pad_orientation;
-               update_pad_rotation();
-       }
-
        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());
+                       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 & pitch != AltosLib.MISSING && rotation != null) {
+                       if (t > 0 && pitch != AltosLib.MISSING && rotation != null) {
                                rotation.rotate(t, pitch, yaw, roll);
                                orient.set_computed(rotation.tilt(), time);
                        }
@@ -958,8 +941,8 @@ public class AltosState extends AltosDataListener {
 
        public void set_gyro(double roll, double pitch, double yaw) {
                gyro_roll = roll;
-               gyro_pitch = roll;
-               gyro_roll = roll;
+               gyro_pitch = pitch;
+               gyro_roll = yaw;
                update_orient();
        }