altosuilib, altoslib: Add azimuth and compass to reports

Here, azimuth means change in horizontal plane from launch.
Compass is reporting mag field strength in X/Y plane

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

diff --git a/altoslib/AltosFlightSeries.java b/altoslib/AltosFlightSeries.java
index cfd8a0bff2bb79aa37b39b2b108d6e406bc3ebd5..07ef74be501ac49b3f867b0a972c1cc8edeb2ae0 100644 (file)
--- a/altoslib/AltosFlightSeries.java
+++ b/altoslib/AltosFlightSeries.java
@@ -380,8 +380,10 @@ public class AltosFlightSeries extends AltosDataListener {
        }
 
        public AltosTimeSeries orient_series;
+       public AltosTimeSeries azimuth_series;
 
        public static final String orient_name = "Tilt Angle";
+       public static final String azimuth_name = "Azimuth Angle";
 
        private void compute_orient() {
 
@@ -408,6 +410,9 @@ public class AltosFlightSeries extends AltosDataListener {
                orient_series = add_series(orient_name, AltosConvert.orient);
                orient_series.add(ground_time, rotation.tilt());
 
+               azimuth_series = add_series(azimuth_name, AltosConvert.orient);
+               azimuth_series.add(ground_time, rotation.azimuth());
+
                for (AltosTimeValue roll_v : gyro_roll) {
                        double  time = roll_v.time;
                        double  dt = time - prev_time;
@@ -419,6 +424,7 @@ public class AltosFlightSeries extends AltosDataListener {
 
                                rotation.rotate(pitch, yaw, roll);
                                orient_series.add(time, rotation.tilt());
+                               azimuth_series.add(time, rotation.azimuth());
                        }
                        prev_time = time;
                }
@@ -622,8 +628,10 @@ public class AltosFlightSeries extends AltosDataListener {
        public static final String mag_along_name = "Magnetic Field Along";
        public static final String mag_across_name = "Magnetic Field Across";
        public static final String mag_through_name = "Magnetic Field Through";
+       public static final String mag_total_name = "Magnetic Field Strength";
+       public static final String compass_name = "Compass";
 
-       public AltosTimeSeries mag_along, mag_across, mag_through;
+       public AltosTimeSeries mag_along, mag_across, mag_through, mag_total, compass;
 
        public  void set_accel(double along, double across, double through) {
                if (accel_along == null) {
@@ -665,10 +673,14 @@ public class AltosFlightSeries extends AltosDataListener {
                        mag_along = add_series(mag_along_name, AltosConvert.magnetic_field);
                        mag_across = add_series(mag_across_name, AltosConvert.magnetic_field);
                        mag_through = add_series(mag_through_name, AltosConvert.magnetic_field);
+                       mag_total = add_series(mag_total_name, AltosConvert.magnetic_field);
+                       compass = add_series(compass_name, AltosConvert.orient);
                }
                mag_along.add(time(), along);
                mag_across.add(time(), across);
                mag_through.add(time(), through);
+               mag_total.add(time(), Math.sqrt(along * along + across * across + through *through));
+               compass.add(time(), Math.atan2(across, through) * 180 / Math.PI);
        }
 
        public void set_orient(double orient) {

diff --git a/altoslib/AltosGauss.java b/altoslib/AltosGauss.java
index 0eed3facd99093d1751f0b44dd3845d918919108..3e6f2fa21c1dae611413e39ec90307c00d181f32 100644 (file)
--- a/altoslib/AltosGauss.java
+++ b/altoslib/AltosGauss.java
@@ -45,9 +45,9 @@ public class AltosGauss extends AltosUnits {
        public AltosGauss() {
                range_metric = new AltosUnitsRange[1];
 
-               range_metric[0] = new AltosUnitsRange(0, "γ", "nano tesla") {
+               range_metric[0] = new AltosUnitsRange(0, "µT", "microtesla") {
                                double value(double v) {
-                                       return v * 100000;
+                                       return v * 100;
                                }
                                int show_fraction(int width) {
                                        return width / 9;

diff --git a/altoslib/AltosRotation.java b/altoslib/AltosRotation.java
index 54b3b1f939a46d1d12fff739fe8b658994afd035..fb84969ce7f8079537b20df704087d34edae357f 100644 (file)
--- a/altoslib/AltosRotation.java
+++ b/altoslib/AltosRotation.java
@@ -29,7 +29,7 @@ public class AltosRotation extends AltosQuaternion {
         *
         * 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.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²
@@ -50,6 +50,34 @@ public class AltosRotation extends AltosQuaternion {
                return tilt;
        }
 
+       /* Compute azimuth angle from a reference line pointing out the side
+        * of the airframe
+        *
+        * rot = ao_rotation * x_axis * ao_rotation°
+        * rot = ao_rotation * (0,1,0,0) * ao_rotation°
+        *     = (-a.x, a.r, a.z, -a.y) * (a.r, -a.x, -a.y, -a.z) . x
+        *     = (-a.x * -a.x) + (a.r * a.r) + (a.z * -a.z) - (-a.y * -a.y)
+        *     = a.x² + a.r² - a.z² - a.y²
+        *
+        *     = (-a.x, a.r, a.z, -a.y) * (a.r, -a.x, -a.y, -a.z) . y
+        *     = (-a.x * -a.y) - (a.r * -a.z) + (a.z * a.r) + (-a.y * -a.x)
+        *     = a.x * a.y + a.r * a.z + a.z * a.r + a.y * a.x
+        *
+        * The X value will be the cosine of the rotation. The Y value will be the
+        * sine of the rotation; use the sign of that to figure out which direction from
+        * zero we've headed
+        */
+
+       public double azimuth() {
+               double rotx = rotation.x * rotation.x + rotation.r * rotation.r - rotation.z * rotation.z - rotation.y * rotation.y;
+               double roty = rotation.x * rotation.y + rotation.r * rotation.z + rotation.z * rotation.r + rotation.y * rotation.x;
+
+               double az = Math.acos(rotx) * 180.0 / Math.PI;
+               if (roty < 0)
+                       return -az;
+               return az;
+       }
+
        /* Given euler rotations in three axes, perform a combined rotation using
         * quaternions
         */

diff --git a/altosuilib/AltosGraph.java b/altosuilib/AltosGraph.java
index 60eae96247c787dc8f96ad1350371d3db3282b7e..5236008f32a395aff091496d5cdeef6321f93d0e 100644 (file)
--- a/altosuilib/AltosGraph.java
+++ b/altosuilib/AltosGraph.java
@@ -45,6 +45,8 @@ public class AltosGraph extends AltosUIGraph {
        static final private AltosUILineStyle accel_color = new AltosUILineStyle();
        static final private AltosUILineStyle vert_accel_color = new AltosUILineStyle();
        static final private AltosUILineStyle orient_color = new AltosUILineStyle();
+       static final private AltosUILineStyle azimuth_color = new AltosUILineStyle();
+       static final private AltosUILineStyle compass_color = new AltosUILineStyle();
 
        static final private AltosUILineStyle gps_height_color = new AltosUILineStyle();
        static final private AltosUILineStyle altitude_color = new AltosUILineStyle();
@@ -84,6 +86,7 @@ public class AltosGraph extends AltosUIGraph {
        static final private AltosUILineStyle mag_along_color = new AltosUILineStyle();
        static final private AltosUILineStyle mag_across_color = new AltosUILineStyle();
        static final private AltosUILineStyle mag_through_color = new AltosUILineStyle();
+       static final private AltosUILineStyle mag_total_color = new AltosUILineStyle();
 
        static AltosUnits dop_units = null;
        static AltosUnits tick_units = null;
@@ -114,7 +117,7 @@ public class AltosGraph extends AltosUIGraph {
                dbm_axis = newAxis("Signal Strength", null, dbm_color, 0);
 
                gyro_axis = newAxis("Rotation Rate", AltosConvert.rotation_rate, gyro_roll_color, 0);
-               orient_axis = newAxis("Tilt Angle", AltosConvert.orient, orient_color, 0);
+               orient_axis = newAxis("Angle", AltosConvert.orient, orient_color, 0);
                mag_axis = newAxis("Magnetic Field", AltosConvert.magnetic_field, mag_along_color, 0);
                course_axis = newAxis("Course", AltosConvert.orient, gps_course_color, 0);
                dop_axis = newAxis("Dilution of Precision", dop_units, gps_pdop_color, 0);
@@ -317,11 +320,26 @@ public class AltosGraph extends AltosUIGraph {
                                            false,
                                            mag_axis);
 
+               flight_series.register_axis(AltosUIFlightSeries.mag_total_name,
+                                           mag_total_color,
+                                           false,
+                                           mag_axis);
+
                flight_series.register_axis(AltosUIFlightSeries.orient_name,
                                            orient_color,
                                            false,
                                            orient_axis);
 
+               flight_series.register_axis(AltosUIFlightSeries.azimuth_name,
+                                           azimuth_color,
+                                           false,
+                                           orient_axis);
+
+               flight_series.register_axis(AltosUIFlightSeries.compass_name,
+                                           compass_color,
+                                           false,
+                                           orient_axis);
+
                flight_series.register_axis(AltosUIFlightSeries.thrust_name,
                                            accel_color,
                                            true,

diff --git a/altosuilib/AltosInfoTable.java b/altosuilib/AltosInfoTable.java
index 6bfe0cf51b20d59c787fab8aebec58c83a7c7742..72d4fb56275e423b1bb16a23c8ddf7b5f6840c3a 100644 (file)
--- a/altosuilib/AltosInfoTable.java
+++ b/altosuilib/AltosInfoTable.java
@@ -280,9 +280,10 @@ public class AltosInfoTable extends JTable implements AltosFlightDisplay, Hierar
                        info_add_row(3, "Gyro yaw", "%8.1f °/s", state.gyro_yaw());
                        if (state.mag_along() != AltosLib.MISSING) {
                                /* Report mag in nanoteslas (1 G = 100000 nT (or γ)) */
-                               info_add_row(3, "Mag along", "%8.1f γ", state.mag_along() * 100000.0);
-                               info_add_row(3, "Mag across", "%8.1f γ", state.mag_across() * 100000.0);
-                               info_add_row(3, "Mag Through", "%8.1f γ", state.mag_through() * 100000.0);
+                               info_add_row(3, "Mag along", "%8.1f µT", state.mag_along() * 100.0);
+                               info_add_row(3, "Mag across", "%8.1f µT", state.mag_across() * 100.0);
+                               info_add_row(3, "Mag Through", "%8.1f µT", state.mag_through() * 100.0);
+                               info_add_row(3, "Mag Bearing", "%8.1f°", Math.atan2(state.mag_across(), state.mag_through()) * 180/Math.PI);
                        }
                }