From: Keith Packard Date: Tue, 25 Feb 2020 19:55:36 +0000 (-0800) Subject: altosuilib, altoslib: Add azimuth and compass to reports X-Git-Tag: 1.9.2~2^2~8 X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=0ab2332358eb0990cb55074001b675d390eae2ca 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 --- diff --git a/altoslib/AltosFlightSeries.java b/altoslib/AltosFlightSeries.java index cfd8a0bf..07ef74be 100644 --- 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 0eed3fac..3e6f2fa2 100644 --- 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 54b3b1f9..fb84969c 100644 --- 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 60eae962..5236008f 100644 --- 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 6bfe0cf5..72d4fb56 100644 --- 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); } }