altoslib: Compute orientation from eeprom data files

This was lost in the AltosFlightSeries transformation.

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

diff --git a/altoslib/AltosCalData.java b/altoslib/AltosCalData.java
index 2eff6ac1188610753c5b38c212183fbf2f0ac405..49b90ce7d5ff9a58f13f7949e2a2f8dd94d0cccb 100644 (file)
--- a/altoslib/AltosCalData.java
+++ b/altoslib/AltosCalData.java
@@ -313,25 +313,27 @@ public class AltosCalData {
                        gyro_zero_roll = roll;
                        gyro_zero_pitch = pitch;
                        gyro_zero_yaw = yaw;
                        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;
                }
        }
 
        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;
        }
 
        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;
        }
 
        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) {
        }
 
        private double gyro_zero_overflow(double first) {


@@ -340,13 +342,25 @@ public class AltosCalData {
                        v = Math.ceil(v);
                else
                        v = Math.floor(v);
                        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;
        }
 
                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) {
        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) {
        }
 
        public double mag_along(double along) {

diff --git a/altoslib/AltosConvert.java b/altoslib/AltosConvert.java
index a252abdf754345ef278ae8f5b606b7d8f8fe7ab4..0d25c6d76d0eb50061a4938c4f5d9ed418a83524 100644 (file)
--- a/altoslib/AltosConvert.java
+++ b/altoslib/AltosConvert.java
@@ -184,6 +184,18 @@ public class AltosConvert {
                return altitude;
        }
 
                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)
        {
        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) {
        }
 
        public static double acceleration_from_sensor(double sensor, double plus_g, double minus_g, double ground) {

+

                if (sensor == AltosLib.MISSING)
                        return AltosLib.MISSING;
 
                if (sensor == AltosLib.MISSING)
                        return AltosLib.MISSING;
 

diff --git a/altoslib/AltosEepromRecordMega.java b/altoslib/AltosEepromRecordMega.java
index 0abc3fe73865c64bae21ec23d5f39c0f0fee47e7..18d435afd7f582c73658c1f7cb2619dcc239188a 100644 (file)
--- a/altoslib/AltosEepromRecordMega.java
+++ b/altoslib/AltosEepromRecordMega.java
@@ -161,11 +161,13 @@ public class AltosEepromRecordMega extends AltosEepromRecord {
                                         cal_data.mag_through(mag_through));
 
 
                                         cal_data.mag_through(mag_through));
 
 

+                       final double lsb_per_g = 1920.0/105.5;
+

                        double acceleration = AltosConvert.acceleration_from_sensor(
                                accel(),
                        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;
 
                        listener.set_acceleration(acceleration);
                        break;

diff --git a/altoslib/AltosFlightSeries.java b/altoslib/AltosFlightSeries.java
index 0b60fdf586dcf1c41ecdd60f61960415ee323c5b..dad066d7ba7853ed36c1d91b7d4049ca6ae7952d 100644 (file)
--- a/altoslib/AltosFlightSeries.java
+++ b/altoslib/AltosFlightSeries.java
@@ -304,6 +304,49 @@ public class AltosFlightSeries extends AltosDataListener {
                        add_series(speed_series);
        }
 
                        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";
        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);
        }
 
                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) {
        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) {
        }
 
        public  void set_gyro(double roll, double pitch, double yaw) {


@@ -524,10 +577,6 @@ public class AltosFlightSeries extends AltosDataListener {
                mag_through.add(time(), through);
        }
 
                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);
        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() {
        }
 
        public void finish() {

+               compute_orient();

                compute_speed();
                compute_accel();
                compute_height();
                compute_speed();
                compute_accel();
                compute_height();

diff --git a/altoslib/AltosIMU.java b/altoslib/AltosIMU.java
index 20a2a413da4f9499a7f1ce052dd52f882440b0c9..b434e02e415d6f77f912d9571b6f2aaaf03461b7 100644 (file)
--- a/altoslib/AltosIMU.java
+++ b/altoslib/AltosIMU.java
@@ -36,10 +36,12 @@ public class AltosIMU implements Cloneable {
                return counts / counts_per_g * AltosConvert.gravity;
        }
 
                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) {
        }
 
        public boolean parse_string(String line) {

diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index 5abc0c2d41a5a6e666039052e784f113f4b6a415..caa1cb4adeb30d8347ca2368a4c4d69f5ae978a4 100644 (file)
--- a/altoslib/AltosState.java
+++ b/altoslib/AltosState.java
@@ -22,8 +22,6 @@
 
 package org.altusmetrum.altoslib_11;
 
 
 package org.altusmetrum.altoslib_11;
 

-import java.io.*;
-

 public class AltosState extends AltosDataListener {
 
        public static final int set_position = 1;
 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;
 
                accel_ground_across = AltosLib.MISSING;
                accel_ground_through = AltosLib.MISSING;
 

-               pad_orientation = AltosLib.MISSING;
-

                set_npad(0);
                ngps = 0;
 
                set_npad(0);
                ngps = 0;
 


@@ -903,16 +899,14 @@ public class AltosState extends AltosDataListener {
        public AltosRotation    rotation;
        public AltosRotation    ground_rotation;
 
        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() {
        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),
                        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);
                }
                        ground_rotation = rotation;
                        orient.set_computed(rotation.tilt(), time);
                }


@@ -925,28 +919,17 @@ public class AltosState extends AltosDataListener {
                update_pad_rotation();
        }
 
                update_pad_rotation();
        }
 

-       public void set_pad_orientation(int pad_orientation) {
-               this.pad_orientation = pad_orientation;
-               update_pad_rotation();
-       }
-

        public double   last_imu_time;
 
        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;
 
        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);
                        }
                                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;
 
        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();
        }
 
                update_orient();
        }