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>
}
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() {
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;
rotation.rotate(pitch, yaw, roll);
orient_series.add(time, rotation.tilt());
+ azimuth_series.add(time, rotation.azimuth());
}
prev_time = time;
}
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) {
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) {
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;
*
* 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²
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
*/
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();
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;
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);
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,
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);
}
}