}
void write_advanced(AltosState state) {
- AltosIMU imu = state.imu;
- AltosMag mag = state.mag;
-
- if (imu == null)
- imu = new AltosIMU();
- if (mag == null)
- mag = new AltosMag();
out.printf("%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7.2f",
- imu.accel_x, imu.accel_y, imu.accel_z,
- imu.gyro_x, imu.gyro_y, imu.gyro_z,
- mag.x, mag.y, mag.z);
+ state.accel_along(), state.accel_across(), state.accel_through(),
+ state.gyro_roll(), state.gyro_pitch(), state.gyro_yaw(),
+ state.mag_along(), state.mag_across(), state.mag_through());
}
void write_gps_header() {
has_basic = true;
if (state.battery_voltage != AltosLib.MISSING)
has_battery = true;
- if (state.imu != null || state.mag != null)
+ if (state.accel_across() != AltosLib.MISSING)
has_advanced = true;
if (state.gps != null) {
has_gps = true;
public int tracker_motion;
public int tracker_interval;
+ /* HAS_GYRO */
+ public int accel_zero_along, accel_zero_across, accel_zero_through;
+
public static String get_string(String line, String label) throws ParseException {
if (line.startsWith(label)) {
String quoted = line.substring(label.length()).trim();
storage_size = -1;
storage_erase_unit = -1;
stored_flight = 0;
+
+ accel_zero_along = -1;
+ accel_zero_across = -1;
+ accel_zero_through = -1;
}
public void parse_line(String line) {
/* Log listing replies */
try { get_int(line, "flight"); stored_flight++; } catch (Exception e) {}
+
+ /* HAS_GYRO */
+ try {
+ if (line.startsWith("IMU call along")) {
+ String[] bits = line.split("\\s+");
+ if (bits.length >= 8) {
+ accel_zero_along = Integer.parseInt(bits[3]);
+ accel_zero_across = Integer.parseInt(bits[5]);
+ accel_zero_through = Integer.parseInt(bits[7]);
+ }
+ }
+ } catch (Exception e) {}
}
public AltosConfigData() {
if (tracker_motion >= 0 && tracker_interval >= 0)
link.printf("c t %d %d\n", tracker_motion, tracker_interval);
+ /* HAS_GYRO */
+ /* UI doesn't support accel cal */
+
link.printf("c w\n");
link.flush_output();
}
public int cmd;
public String data;
- public int config_a, config_b;
+ public int config_a, config_b, config_c;
public boolean last;
public boolean valid;
state.make_baro();
state.baro.crc = config_a;
break;
+ case AltosLib.AO_LOG_IMU_CAL:
+ state.set_accel_zero(config_a, config_b, config_c);
+ break;
case AltosLib.AO_LOG_SOFTWARE_VERSION:
state.set_firmware_version(data);
break;
case AltosLib.AO_LOG_APOGEE_LOCKOUT:
case AltosLib.AO_LOG_RADIO_RATE:
case AltosLib.AO_LOG_IGNITE_MODE:
+ break;
case AltosLib.AO_LOG_PAD_ORIENTATION:
+ state.set_pad_orientation(config_a);
+ break;
case AltosLib.AO_LOG_RADIO_ENABLE:
case AltosLib.AO_LOG_AES_KEY:
case AltosLib.AO_LOG_APRS:
case AltosLib.AO_LOG_BARO_CRC:
out.printf ("# Baro crc: %d\n", config_a);
break;
+ case AltosLib.AO_LOG_IMU_CAL:
+ out.printf ("# IMU cal: %d %d %d\n", config_a, config_b, config_c);
+ break;
case AltosLib.AO_LOG_FREQUENCY:
case AltosLib.AO_LOG_APOGEE_LOCKOUT:
case AltosLib.AO_LOG_RADIO_RATE:
case AltosLib.AO_LOG_IGNITE_MODE:
+ break;
case AltosLib.AO_LOG_PAD_ORIENTATION:
+ out.printf("# Pad orientation: %d\n", config_a);
+ break;
case AltosLib.AO_LOG_RADIO_ENABLE:
case AltosLib.AO_LOG_AES_KEY:
case AltosLib.AO_LOG_APRS:
cmd = AltosLib.AO_LOG_INVALID;
data = tokens[2];
}
+ } else if (tokens[0].equals("IMU") && tokens[1].equals("cal")) {
+ cmd = AltosLib.AO_LOG_IMU_CAL;
+ config_a = Integer.parseInt(tokens[3]);
+ config_b = Integer.parseInt(tokens[5]);
+ config_c = Integer.parseInt(tokens[7]);
+ } else if (tokens[0].equals("Pad") && tokens[1].equals("orientation:")) {
+ cmd = AltosLib.AO_LOG_PAD_ORIENTATION;
+ config_a = Integer.parseInt(tokens[2]);
} else
valid = false;
} catch (Exception e) {
state.set_flight(flight());
state.set_ground_accel(ground_accel());
state.set_ground_pressure(ground_pres());
+ state.set_accel_ground(ground_accel_along(),
+ ground_accel_across(),
+ ground_accel_through());
+ state.set_gyro_zero(ground_roll() / 512.0,
+ ground_pitch() / 512.0,
+ ground_yaw() / 512.0);
break;
case AltosLib.AO_LOG_STATE:
state.set_tick(tick);
state.set_tick(tick);
state.set_ms5607(pres(), temp());
- AltosIMU imu = new AltosIMU();
- imu.accel_x = AltosIMU.convert_accel(accel_x());
- imu.accel_y = AltosIMU.convert_accel(accel_y());
- imu.accel_z = AltosIMU.convert_accel(accel_z());
+ AltosIMU imu = new AltosIMU(accel_y(), /* along */
+ accel_x(), /* across */
+ accel_z(), /* through */
+ gyro_y(), /* roll */
+ gyro_x(), /* pitch */
+ gyro_z()); /* yaw */
- imu.gyro_x = AltosIMU.convert_gyro(gyro_x());
- imu.gyro_y = AltosIMU.convert_gyro(gyro_y());
- imu.gyro_z = AltosIMU.convert_gyro(gyro_z());
- state.imu = imu;
+ if (log_format == AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD)
+ state.check_imu_wrap(imu);
- AltosMag mag = new AltosMag();
- mag.x = AltosMag.convert_gauss(mag_x());
- mag.y = AltosMag.convert_gauss(mag_y());
- mag.z = AltosMag.convert_gauss(mag_z());
+ state.set_imu(imu);
- state.mag = mag;
+ state.set_mag(new AltosMag(mag_x(),
+ mag_y(),
+ mag_z()));
state.set_accel(accel());
import java.util.concurrent.*;
public class AltosIMU implements Cloneable {
- public double accel_x;
- public double accel_y;
- public double accel_z;
+ public int accel_along;
+ public int accel_across;
+ public int accel_through;
- public double gyro_x;
- public double gyro_y;
- public double gyro_z;
+ public int gyro_roll;
+ public int gyro_pitch;
+ public int gyro_yaw;
-/*
- * XXX use ground measurements to adjust values
-
- public double ground_accel_x;
- public double ground_accel_y;
- public double ground_accel_z;
-
- public double ground_gyro_x;
- public double ground_gyro_y;
- public double ground_gyro_z;
-*/
-
- public static int counts_per_g = 2048;
+ public static double counts_per_g = 2048.0;
- public static double convert_accel(int counts) {
- return (double) counts / (double) counts_per_g * (-AltosConvert.GRAVITATIONAL_ACCELERATION);
+ public static double convert_accel(double counts) {
+ return counts / counts_per_g * (-AltosConvert.GRAVITATIONAL_ACCELERATION);
}
public static double counts_per_degsec = 16.4;
- public static double convert_gyro(int counts) {
- return (double) counts / counts_per_degsec;
+ public static double convert_gyro(double counts) {
+ return counts / counts_per_degsec;
}
public boolean parse_string(String line) {
String[] items = line.split("\\s+");
if (items.length >= 8) {
- accel_x = convert_accel(Integer.parseInt(items[1]));
- accel_y = convert_accel(Integer.parseInt(items[2]));
- accel_z = convert_accel(Integer.parseInt(items[3]));
- gyro_x = convert_gyro(Integer.parseInt(items[5]));
- gyro_y = convert_gyro(Integer.parseInt(items[6]));
- gyro_z = convert_gyro(Integer.parseInt(items[7]));
+ accel_along = Integer.parseInt(items[1]);
+ accel_across = Integer.parseInt(items[2]);
+ accel_through = Integer.parseInt(items[3]);
+ gyro_roll = Integer.parseInt(items[5]);
+ gyro_pitch = Integer.parseInt(items[6]);
+ gyro_yaw = Integer.parseInt(items[7]);
}
return true;
}
public AltosIMU clone() {
AltosIMU n = new AltosIMU();
- n.accel_x = accel_x;
- n.accel_y = accel_y;
- n.accel_z = accel_z;
+ n.accel_along = accel_along;
+ n.accel_across = accel_across;
+ n.accel_through = accel_through;
- n.gyro_x = gyro_x;
- n.gyro_y = gyro_y;
- n.gyro_z = gyro_z;
+ n.gyro_roll = gyro_roll;
+ n.gyro_pitch = gyro_pitch;
+ n.gyro_yaw = gyro_yaw;
return n;
}
}
public AltosIMU() {
- accel_x = AltosLib.MISSING;
- accel_y = AltosLib.MISSING;
- accel_z = AltosLib.MISSING;
+ accel_along = AltosLib.MISSING;
+ accel_across = AltosLib.MISSING;
+ accel_through = AltosLib.MISSING;
+
+ gyro_roll = AltosLib.MISSING;
+ gyro_pitch = AltosLib.MISSING;
+ gyro_yaw = AltosLib.MISSING;
+ }
+
+ public AltosIMU(int accel_along, int accel_across, int accel_through,
+ int gyro_roll, int gyro_pitch, int gyro_yaw) {
+
+ this.accel_along = accel_along;
+ this.accel_across = accel_across;
+ this.accel_through = accel_through;
- gyro_x = AltosLib.MISSING;
- gyro_y = AltosLib.MISSING;
- gyro_z = AltosLib.MISSING;
+ this.gyro_roll = gyro_roll;
+ this.gyro_pitch = gyro_pitch;
+ this.gyro_yaw = gyro_yaw;
}
public AltosIMU(AltosLink link) throws InterruptedException, TimeoutException {
public static final int AO_LOG_BARO_TREF = 3006;
public static final int AO_LOG_BARO_TEMPSENS = 3007;
public static final int AO_LOG_BARO_CRC = 3008;
+ public static final int AO_LOG_IMU_CAL = 3009;
public static final int AO_LOG_SOFTWARE_VERSION = 9999;
import java.util.concurrent.*;
public class AltosMag implements Cloneable {
- public double x;
- public double y;
- public double z;
+ public int along;
+ public int across;
+ public int through;
public static double counts_per_gauss = 1090;
- public static double convert_gauss(int counts) {
- return (double) counts / counts_per_gauss;
+ public static double convert_gauss(double counts) {
+ return counts / counts_per_gauss;
}
public boolean parse_string(String line) {
// if (line.startsWith("Syntax error")) {
-// x = y = z = 0;
+// along = across = through = 0;
// return true;
// }
String[] items = line.split("\\s+");
if (items.length >= 6) {
- x = convert_gauss(Integer.parseInt(items[1]));
- y = convert_gauss(Integer.parseInt(items[3]));
- z = convert_gauss(Integer.parseInt(items[5]));
+ along = Integer.parseInt(items[1]);
+ across = Integer.parseInt(items[3]);
+ through = Integer.parseInt(items[5]);
}
return true;
}
public AltosMag clone() {
AltosMag n = new AltosMag();
- n.x = x;
- n.y = y;
- n.z = z;
+ n.along = along;
+ n.across = across;
+ n.through = through;
return n;
}
public AltosMag() {
- x = AltosLib.MISSING;
- y = AltosLib.MISSING;
- z = AltosLib.MISSING;
+ along = AltosLib.MISSING;
+ across = AltosLib.MISSING;
+ through = AltosLib.MISSING;
+ }
+
+ public AltosMag(int along, int across, int through) {
+ this.along = along;
+ this.across = across;
+ this.through = through;
}
static public void update_state(AltosState state, AltosLink link, AltosConfigData config_data) throws InterruptedException {
--- /dev/null
+/*
+ * Copyright © 2014 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+package org.altusmetrum.altoslib_5;
+
+public class AltosQuaternion {
+ double r; /* real bit */
+ double x, y, z; /* imaginary bits */
+
+ public AltosQuaternion multiply(AltosQuaternion b) {
+ return new AltosQuaternion(
+ this.r * b.r - this.x * b.x - this.y * b.y - this.z * b.z,
+ this.r * b.x + this.x * b.r + this.y * b.z - this.z * b.y,
+ this.r * b.y - this.x * b.z + this.y * b.r + this.z * b.x,
+ this.r * b.z + this.x * b.y - this.y * b.x + this.z * b.r);
+ }
+
+ public AltosQuaternion conjugate() {
+ return new AltosQuaternion(
+ this.r,
+ -this.x,
+ -this.y,
+ -this.z);
+ }
+
+ public double normal() {
+ return (this.r * this.r +
+ this.x * this.x +
+ this.y * this.y +
+ this.z * this.z);
+ }
+
+ public AltosQuaternion scale(double b) {
+ return new AltosQuaternion(
+ this.r * b,
+ this.x * b,
+ this.y * b,
+ this.z * b);
+ }
+
+ public AltosQuaternion normalize() {
+ double n = normal();
+ if (n <= 0)
+ return this;
+ return scale(1/Math.sqrt(n));
+ }
+
+ public double dot(AltosQuaternion b) {
+ return (this.r * b.r +
+ this.x * b.x +
+ this.y * b.y +
+ this.z * b.z);
+ }
+
+ public AltosQuaternion rotate(AltosQuaternion b) {
+ return (b.multiply(this)).multiply(b.conjugate());
+ }
+
+ public AltosQuaternion vectors_to_rotation(AltosQuaternion b) {
+ /*
+ * The cross product will point orthogonally to the two
+ * vectors, forming our rotation axis. The length will be
+ * sin(θ), so these values are already multiplied by that.
+ */
+
+ double x = this.y * b.z - this.z * b.y;
+ double y = this.z * b.x - this.x * b.z;
+ double z = this.x * b.y - this.y * b.x;
+
+ double s_2 = x*x + y*y + z*z;
+ double s = Math.sqrt(s_2);
+
+ /* cos(θ) = a · b / (|a| |b|).
+ *
+ * a and b are both unit vectors, so the divisor is one
+ */
+ double c = this.x*b.x + this.y*b.y + this.z*b.z;
+
+ double c_half = Math.sqrt ((1 + c) / 2);
+ double s_half = Math.sqrt ((1 - c) / 2);
+
+ /*
+ * Divide out the sine factor from the
+ * cross product, then multiply in the
+ * half sine factor needed for the quaternion
+ */
+ double s_scale = s_half / s;
+
+ AltosQuaternion r = new AltosQuaternion(c_half,
+ x * s_scale,
+ y * s_scale,
+ z * s_scale);
+ return r.normalize();
+ }
+
+ public AltosQuaternion(double r, double x, double y, double z) {
+ this.r = r;
+ this.x = x;
+ this.y = y;
+ this.z = z;
+ }
+
+ public AltosQuaternion(AltosQuaternion q) {
+ this.r = q.r;
+ this.x = q.x;
+ this.y = q.y;
+ this.z = q.z;
+ }
+
+ static public AltosQuaternion vector(double x, double y, double z) {
+ return new AltosQuaternion(0, x, y, z);
+ }
+
+ static public AltosQuaternion rotation(double x, double y, double z,
+ double s, double c) {
+ return new AltosQuaternion(c,
+ s*x,
+ s*y,
+ s*z);
+ }
+
+ static public AltosQuaternion zero_rotation() {
+ return new AltosQuaternion(1, 0, 0, 0);
+ }
+
+ static public AltosQuaternion half_euler(double x, double y, double z) {
+ double s_x = Math.sin(x), c_x = Math.cos(x);
+ double s_y = Math.sin(y), c_y = Math.cos(y);
+ double s_z = Math.sin(z), c_z = Math.cos(z);;
+
+ return new AltosQuaternion(c_x * c_y * c_z + s_x * s_y * s_z,
+ s_x * c_y * c_z - c_x * s_y * s_z,
+ c_x * s_y * c_z + s_x * c_y * s_z,
+ c_x * c_y * s_z - s_x * s_y * c_z);
+ }
+}
--- /dev/null
+/*
+ * Copyright © 2014 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+package org.altusmetrum.altoslib_5;
+
+public class AltosRotation {
+ private AltosQuaternion rotation;
+
+ public double tilt() {
+ double rotz = rotation.z * rotation.z - rotation.y * rotation.y - rotation.x * rotation.x + rotation.r * rotation.r;
+
+ double tilt = Math.acos(rotz) * 180.0 / Math.PI;
+ return tilt;
+ }
+
+ public void rotate(double dt, double x, double y, double z) {
+ AltosQuaternion rot = AltosQuaternion.half_euler(x * dt, y * dt, z * dt);
+ rotation = rot.multiply(rotation).normalize();
+ }
+
+ /* Clone an existing rotation value */
+ public AltosRotation (AltosRotation old) {
+ this.rotation = new AltosQuaternion(old.rotation);
+ }
+
+ /* Create a new rotation value given an acceleration vector pointing down */
+ public AltosRotation(double x,
+ double y,
+ double z,
+ int pad_orientation) {
+ AltosQuaternion orient = AltosQuaternion.vector(x, y, z).normalize();
+ double sky = pad_orientation == 0 ? 1 : -1;
+ AltosQuaternion up = new AltosQuaternion(0, 0, 0, sky);
+ rotation = up.vectors_to_rotation(orient);
+ }
+}
public int boost_tick;
class AltosValue {
- private double value;
- private double prev_value;
+ double value;
+ double prev_value;
private double max_value;
private double set_time;
private double prev_set_time;
}
class AltosCValue {
- AltosValue measured;
- AltosValue computed;
+
+ class AltosIValue extends AltosValue {
+ boolean can_max() {
+ return c_can_max();
+ }
+ };
+
+ public AltosIValue measured;
+ public AltosIValue computed;
+
+ boolean can_max() { return true; }
+
+ boolean c_can_max() { return can_max(); }
double value() {
double v = measured.value();
}
AltosCValue() {
- measured = new AltosValue();
- computed = new AltosValue();
+ measured = new AltosIValue();
+ computed = new AltosIValue();
}
}
return acceleration.max();
}
- public AltosValue orient;
+ public AltosCValue orient;
public void set_orient(double new_orient) {
- orient.set(new_orient, time);
+ orient.set_measured(new_orient, time);
}
public double orient() {
pressure = new AltosPressure();
speed = new AltosSpeed();
acceleration = new AltosAccel();
- orient = new AltosValue();
+ orient = new AltosCValue();
temperature = AltosLib.MISSING;
battery_voltage = AltosLib.MISSING;
gps_pending = false;
imu = null;
+ last_imu_time = AltosLib.MISSING;
+ rotation = null;
+ ground_rotation = null;
+
mag = null;
+ accel_zero_along = AltosLib.MISSING;
+ accel_zero_across = AltosLib.MISSING;
+ accel_zero_through = AltosLib.MISSING;
+
+ accel_ground_along = AltosLib.MISSING;
+ accel_ground_across = AltosLib.MISSING;
+ accel_ground_through = AltosLib.MISSING;
+
+ pad_orientation = AltosLib.MISSING;
+
+ gyro_zero_roll = AltosLib.MISSING;
+ gyro_zero_pitch = AltosLib.MISSING;
+ gyro_zero_yaw = AltosLib.MISSING;
set_npad(0);
ngps = 0;
imu = old.imu.clone();
else
imu = null;
+ last_imu_time = old.last_imu_time;
+
+ if (old.rotation != null)
+ rotation = new AltosRotation (old.rotation);
+
+ if (old.ground_rotation != null) {
+ ground_rotation = new AltosRotation(old.ground_rotation);
+ }
+
+ accel_zero_along = old.accel_zero_along;
+ accel_zero_across = old.accel_zero_across;
+ accel_zero_through = old.accel_zero_through;
+
+ accel_ground_along = old.accel_ground_along;
+ accel_ground_across = old.accel_ground_across;
+ accel_ground_through = old.accel_ground_through;
+ pad_orientation = old.pad_orientation;
+
+ gyro_zero_roll = old.gyro_zero_roll;
+ gyro_zero_pitch = old.gyro_zero_pitch;
+ gyro_zero_yaw = old.gyro_zero_yaw;
if (old.mag != null)
mag = old.mag.clone();
}
}
+
+ public double accel_zero_along;
+ public double accel_zero_across;
+ public double accel_zero_through;
+
+ public AltosRotation rotation;
+ public AltosRotation ground_rotation;
+
+ public void set_accel_zero(double zero_along, double zero_across, double zero_through) {
+ if (zero_along != AltosLib.MISSING) {
+ accel_zero_along = zero_along;
+ accel_zero_across = zero_across;
+ accel_zero_through = zero_through;
+ }
+ }
+
+ 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) {
+ rotation = new AltosRotation(AltosIMU.convert_accel(accel_ground_across - accel_zero_across),
+ AltosIMU.convert_accel(accel_ground_through - accel_zero_through),
+ AltosIMU.convert_accel(accel_ground_along - accel_zero_along),
+ pad_orientation);
+ ground_rotation = rotation;
+ orient.set_computed(rotation.tilt(), time);
+ }
+ }
+
+ public void set_accel_ground(double ground_along, double ground_across, double ground_through) {
+ accel_ground_along = ground_along;
+ accel_ground_across = ground_across;
+ accel_ground_through = ground_through;
+ update_pad_rotation();
+ }
+
+ public void set_pad_orientation(int pad_orientation) {
+ this.pad_orientation = pad_orientation;
+ update_pad_rotation();
+ }
+
+ public double gyro_zero_roll;
+ public double gyro_zero_pitch;
+ public double gyro_zero_yaw;
+
+ public void set_gyro_zero(double roll, double pitch, double yaw) {
+ if (roll != AltosLib.MISSING) {
+ gyro_zero_roll = roll;
+ gyro_zero_pitch = pitch;
+ gyro_zero_yaw = yaw;
+ }
+ }
+
+ 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());
+
+ if (t > 0 & pitch != AltosLib.MISSING && rotation != null) {
+ rotation.rotate(t, pitch, yaw, roll);
+ orient.set_computed(rotation.tilt(), time);
+ }
+ }
+ last_imu_time = time;
+ }
+
public void set_imu(AltosIMU imu) {
if (imu != null)
imu = imu.clone();
this.imu = imu;
+ update_orient();
+ }
+
+ private double gyro_zero_overflow(double first) {
+ double v = first / 128.0;
+ if (v < 0)
+ v = Math.ceil(v);
+ else
+ v = Math.floor(v);
+ return v * 128.0;
+ }
+
+ public void check_imu_wrap(AltosIMU imu) {
+ if (this.imu == null) {
+ gyro_zero_roll += gyro_zero_overflow(imu.gyro_roll);
+ gyro_zero_pitch += gyro_zero_overflow(imu.gyro_pitch);
+ gyro_zero_yaw += gyro_zero_overflow(imu.gyro_yaw);
+ }
+ }
+
+ public double accel_along() {
+ if (imu != null && accel_zero_along != AltosLib.MISSING)
+ return AltosIMU.convert_accel(imu.accel_along - accel_zero_along);
+ return AltosLib.MISSING;
+ }
+
+ public double accel_across() {
+ if (imu != null && accel_zero_across != AltosLib.MISSING)
+ return AltosIMU.convert_accel(imu.accel_across - accel_zero_across);
+ return AltosLib.MISSING;
+ }
+
+ public double accel_through() {
+ if (imu != null && accel_zero_through != AltosLib.MISSING)
+ return AltosIMU.convert_accel(imu.accel_through - accel_zero_through);
+ return AltosLib.MISSING;
+ }
+
+ public double gyro_roll() {
+ if (imu != null && gyro_zero_roll != AltosLib.MISSING) {
+ return AltosIMU.convert_gyro(imu.gyro_roll - gyro_zero_roll);
+ }
+ return AltosLib.MISSING;
+ }
+
+ public double gyro_pitch() {
+ if (imu != null && gyro_zero_pitch != AltosLib.MISSING) {
+ return AltosIMU.convert_gyro(imu.gyro_pitch - gyro_zero_pitch);
+ }
+ return AltosLib.MISSING;
+ }
+
+ public double gyro_yaw() {
+ if (imu != null && gyro_zero_yaw != AltosLib.MISSING) {
+ return AltosIMU.convert_gyro(imu.gyro_yaw - gyro_zero_yaw);
+ }
+ return AltosLib.MISSING;
}
public void set_mag(AltosMag mag) {
this.mag = mag.clone();
}
+ public double mag_along() {
+ if (mag != null)
+ return AltosMag.convert_gauss(mag.along);
+ return AltosLib.MISSING;
+ }
+
+ public double mag_across() {
+ if (mag != null)
+ return AltosMag.convert_gauss(mag.across);
+ return AltosLib.MISSING;
+ }
+
+ public double mag_through() {
+ if (mag != null)
+ return AltosMag.convert_gauss(mag.through);
+ return AltosLib.MISSING;
+ }
+
public AltosMs5607 make_baro() {
if (baro == null)
baro = new AltosMs5607();
state.set_orient(orient);
- AltosIMU imu = new AltosIMU();
-
- imu.accel_x = AltosIMU.convert_accel(accel_x);
- imu.accel_y = AltosIMU.convert_accel(accel_y);
- imu.accel_z = AltosIMU.convert_accel(accel_z);
-
- imu.gyro_x = AltosIMU.convert_gyro(gyro_x);
- imu.gyro_y = AltosIMU.convert_gyro(gyro_y);
- imu.gyro_z = AltosIMU.convert_gyro(gyro_z);
-
- state.imu = imu;
-
- AltosMag mag = new AltosMag();
-
- mag.x = AltosMag.convert_gauss(mag_x);
- mag.y = AltosMag.convert_gauss(mag_y);
- mag.z = AltosMag.convert_gauss(mag_z);
-
- state.mag = mag;
+ state.set_imu(new AltosIMU(accel_y, /* along */
+ accel_x, /* across */
+ accel_z, /* through */
+ gyro_y, /* along */
+ gyro_x, /* across */
+ gyro_z)); /* through */
+
+ state.set_mag(new AltosMag(mag_x, mag_y, mag_z));
}
}
AltosLatitude.java \
AltosLongitude.java \
AltosPyro.java \
- AltosWriter.java
+ AltosWriter.java \
+ AltosQuaternion.java \
+ AltosRotation.java
JAR=altoslib_$(ALTOSLIB_VERSION).jar
}
if (stats.has_imu) {
- addSeries("Acceleration X",
- AltosGraphDataPoint.data_accel_x,
+ addSeries("Acceleration Along",
+ AltosGraphDataPoint.data_accel_along,
AltosConvert.accel,
accel_x_color,
false,
accel_axis);
- addSeries("Acceleration Y",
- AltosGraphDataPoint.data_accel_y,
+ addSeries("Acceleration Across",
+ AltosGraphDataPoint.data_accel_across,
AltosConvert.accel,
accel_y_color,
false,
accel_axis);
- addSeries("Acceleration Z",
- AltosGraphDataPoint.data_accel_z,
+ addSeries("Acceleration Through",
+ AltosGraphDataPoint.data_accel_through,
AltosConvert.accel,
accel_z_color,
false,
accel_axis);
- addSeries("Rotation Rate X",
- AltosGraphDataPoint.data_gyro_x,
+ addSeries("Roll Rate",
+ AltosGraphDataPoint.data_gyro_roll,
gyro_units,
gyro_x_color,
false,
gyro_axis);
- addSeries("Rotation Rate Y",
- AltosGraphDataPoint.data_gyro_y,
+ addSeries("Pitch Rate",
+ AltosGraphDataPoint.data_gyro_pitch,
gyro_units,
gyro_y_color,
false,
gyro_axis);
- addSeries("Rotation Rate Z",
- AltosGraphDataPoint.data_gyro_z,
+ addSeries("Yaw Rate",
+ AltosGraphDataPoint.data_gyro_yaw,
gyro_units,
gyro_z_color,
false,
gyro_axis);
}
if (stats.has_mag) {
- addSeries("Magnetometer X",
- AltosGraphDataPoint.data_mag_x,
+ addSeries("Magnetometer Along",
+ AltosGraphDataPoint.data_mag_along,
mag_units,
mag_x_color,
false,
mag_axis);
- addSeries("Magnetometer Y",
- AltosGraphDataPoint.data_mag_y,
+ addSeries("Magnetometer Across",
+ AltosGraphDataPoint.data_mag_across,
mag_units,
mag_y_color,
false,
mag_axis);
- addSeries("Magnetometer Z",
- AltosGraphDataPoint.data_mag_z,
+ addSeries("Magnetometer Through",
+ AltosGraphDataPoint.data_mag_through,
mag_units,
mag_z_color,
false,
public static final int data_range = 14;
public static final int data_distance = 15;
public static final int data_pressure = 16;
- public static final int data_accel_x = 17;
- public static final int data_accel_y = 18;
- public static final int data_accel_z = 19;
- public static final int data_gyro_x = 20;
- public static final int data_gyro_y = 21;
- public static final int data_gyro_z = 22;
- public static final int data_mag_x = 23;
- public static final int data_mag_y = 24;
- public static final int data_mag_z = 25;
+ public static final int data_accel_along = 17;
+ public static final int data_accel_across = 18;
+ public static final int data_accel_through = 19;
+ public static final int data_gyro_roll = 20;
+ public static final int data_gyro_pitch = 21;
+ public static final int data_gyro_yaw = 22;
+ public static final int data_mag_along = 23;
+ public static final int data_mag_across = 24;
+ public static final int data_mag_through = 25;
public static final int data_orient = 26;
public static final int data_gps_course = 27;
public static final int data_gps_ground_speed = 28;
y = state.pressure();
break;
- case data_accel_x:
- case data_accel_y:
- case data_accel_z:
- case data_gyro_x:
- case data_gyro_y:
- case data_gyro_z:
- AltosIMU imu = state.imu;
- if (imu == null)
- break;
- switch (index) {
- case data_accel_x:
- y = imu.accel_x;
- break;
- case data_accel_y:
- y = imu.accel_y;
- break;
- case data_accel_z:
- y = imu.accel_z;
- break;
- case data_gyro_x:
- y = imu.gyro_x;
- break;
- case data_gyro_y:
- y = imu.gyro_y;
- break;
- case data_gyro_z:
- y = imu.gyro_z;
- break;
- }
+ case data_accel_along:
+ y = state.accel_along();
break;
- case data_mag_x:
- case data_mag_y:
- case data_mag_z:
- AltosMag mag = state.mag;
- if (mag == null)
- break;
- switch (index) {
- case data_mag_x:
- y = mag.x;
- break;
- case data_mag_y:
- y = mag.y;
- break;
- case data_mag_z:
- y = mag.z;
- break;
- }
+ case data_accel_across:
+ y = state.accel_across();
+ break;
+ case data_accel_through:
+ y = state.accel_through();
+ break;
+ case data_gyro_roll:
+ y = state.gyro_roll();
+ break;
+ case data_gyro_pitch:
+ y = state.gyro_pitch();
+ break;
+ case data_gyro_yaw:
+ y = state.gyro_yaw();
+ break;
+ case data_mag_along:
+ y = state.mag_along();
+ break;
+ case data_mag_across:
+ y = state.mag_across();
+ break;
+ case data_mag_through:
+ y = state.mag_through();
break;
case data_orient:
y = state.orient();
projects / fw / altos / commitdiff