From: Keith Packard Date: Sat, 4 Oct 2014 07:11:13 +0000 (-0700) Subject: altoslib: Compute tilt angle from eeprom data X-Git-Tag: bdale-altosdroid~12 X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=00ae706dab6e8fddef4c5730a17c433a022228b7 altoslib: Compute tilt angle from eeprom data This copies the computation of tilt angle from the firmware so that post-flight analysis can also show the data. This change also renames all of the imu values to make them easier to understand: accel gyro axis along roll length of the board across pitch across the board through yaw through the board. Signed-off-by: Keith Packard --- diff --git a/altoslib/AltosCSV.java b/altoslib/AltosCSV.java index 4a9278d9..2357dbc7 100644 --- a/altoslib/AltosCSV.java +++ b/altoslib/AltosCSV.java @@ -162,17 +162,10 @@ public class AltosCSV implements AltosWriter { } 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() { @@ -381,7 +374,7 @@ public class AltosCSV implements AltosWriter { 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; diff --git a/altoslib/AltosConfigData.java b/altoslib/AltosConfigData.java index fc1f2442..a9e863c7 100644 --- a/altoslib/AltosConfigData.java +++ b/altoslib/AltosConfigData.java @@ -90,6 +90,9 @@ public class AltosConfigData implements Iterable { 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(); @@ -266,6 +269,10 @@ public class AltosConfigData implements Iterable { 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) { @@ -361,6 +368,18 @@ public class AltosConfigData implements Iterable { /* 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() { @@ -637,6 +656,9 @@ public class AltosConfigData implements Iterable { 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(); } diff --git a/altoslib/AltosEepromHeader.java b/altoslib/AltosEepromHeader.java index 71030655..0414c37e 100644 --- a/altoslib/AltosEepromHeader.java +++ b/altoslib/AltosEepromHeader.java @@ -25,7 +25,7 @@ public class AltosEepromHeader extends AltosEeprom { 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; @@ -93,6 +93,9 @@ public class AltosEepromHeader extends AltosEeprom { 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; @@ -100,7 +103,10 @@ public class AltosEepromHeader extends AltosEeprom { 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: @@ -177,11 +183,17 @@ public class AltosEepromHeader extends AltosEeprom { 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: @@ -273,6 +285,14 @@ public class AltosEepromHeader extends AltosEeprom { 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) { diff --git a/altoslib/AltosEepromMega.java b/altoslib/AltosEepromMega.java index aaff2410..f9bfb4a7 100644 --- a/altoslib/AltosEepromMega.java +++ b/altoslib/AltosEepromMega.java @@ -149,6 +149,12 @@ public class AltosEepromMega extends AltosEeprom { 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); @@ -158,22 +164,21 @@ public class AltosEepromMega extends AltosEeprom { 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()); diff --git a/altoslib/AltosIMU.java b/altoslib/AltosIMU.java index 89d7def4..d7373f3c 100644 --- a/altoslib/AltosIMU.java +++ b/altoslib/AltosIMU.java @@ -20,36 +20,24 @@ package org.altusmetrum.altoslib_5; 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) { @@ -59,12 +47,12 @@ public class AltosIMU implements Cloneable { 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; } @@ -72,13 +60,13 @@ public class AltosIMU implements Cloneable { 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; } @@ -93,13 +81,25 @@ public class AltosIMU implements Cloneable { } 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 { diff --git a/altoslib/AltosLib.java b/altoslib/AltosLib.java index 92a22ec7..0edc0b43 100644 --- a/altoslib/AltosLib.java +++ b/altoslib/AltosLib.java @@ -74,6 +74,7 @@ public class AltosLib { 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; diff --git a/altoslib/AltosMag.java b/altoslib/AltosMag.java index 690241f1..1fa8877b 100644 --- a/altoslib/AltosMag.java +++ b/altoslib/AltosMag.java @@ -20,19 +20,19 @@ package org.altusmetrum.altoslib_5; 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; // } @@ -42,9 +42,9 @@ public class AltosMag implements Cloneable { 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; } @@ -52,16 +52,22 @@ public class AltosMag implements Cloneable { 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 { diff --git a/altoslib/AltosQuaternion.java b/altoslib/AltosQuaternion.java new file mode 100644 index 00000000..a7125100 --- /dev/null +++ b/altoslib/AltosQuaternion.java @@ -0,0 +1,150 @@ +/* + * Copyright © 2014 Keith Packard + * + * 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); + } +} diff --git a/altoslib/AltosRotation.java b/altoslib/AltosRotation.java new file mode 100644 index 00000000..ad5aca8f --- /dev/null +++ b/altoslib/AltosRotation.java @@ -0,0 +1,50 @@ +/* + * Copyright © 2014 Keith Packard + * + * 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); + } +} diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index 5fce15c4..830e95f3 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -44,8 +44,8 @@ public class AltosState implements Cloneable { 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; @@ -180,8 +180,19 @@ public class AltosState implements Cloneable { } 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(); @@ -263,8 +274,8 @@ public class AltosState implements Cloneable { } AltosCValue() { - measured = new AltosValue(); - computed = new AltosValue(); + measured = new AltosIValue(); + computed = new AltosIValue(); } } @@ -600,10 +611,10 @@ public class AltosState implements Cloneable { 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() { @@ -713,7 +724,7 @@ public class AltosState implements Cloneable { pressure = new AltosPressure(); speed = new AltosSpeed(); acceleration = new AltosAccel(); - orient = new AltosValue(); + orient = new AltosCValue(); temperature = AltosLib.MISSING; battery_voltage = AltosLib.MISSING; @@ -733,7 +744,24 @@ public class AltosState implements Cloneable { 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; @@ -861,6 +889,27 @@ public class AltosState implements Cloneable { 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(); @@ -1112,16 +1161,170 @@ public class AltosState implements Cloneable { } } + + 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(); diff --git a/altoslib/AltosTelemetryMegaSensor.java b/altoslib/AltosTelemetryMegaSensor.java index 1b568c88..bae18d1c 100644 --- a/altoslib/AltosTelemetryMegaSensor.java +++ b/altoslib/AltosTelemetryMegaSensor.java @@ -66,24 +66,13 @@ public class AltosTelemetryMegaSensor extends AltosTelemetryStandard { 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)); } } diff --git a/altoslib/Makefile.am b/altoslib/Makefile.am index 0ad9b7da..2805742e 100644 --- a/altoslib/Makefile.am +++ b/altoslib/Makefile.am @@ -124,7 +124,9 @@ altoslib_JAVA = \ AltosLatitude.java \ AltosLongitude.java \ AltosPyro.java \ - AltosWriter.java + AltosWriter.java \ + AltosQuaternion.java \ + AltosRotation.java JAR=altoslib_$(ALTOSLIB_VERSION).jar diff --git a/altosuilib/AltosGraph.java b/altosuilib/AltosGraph.java index 522eea1e..2e6d428d 100644 --- a/altosuilib/AltosGraph.java +++ b/altosuilib/AltosGraph.java @@ -410,58 +410,58 @@ public class AltosGraph extends AltosUIGraph { } 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, diff --git a/altosuilib/AltosGraphDataPoint.java b/altosuilib/AltosGraphDataPoint.java index 56dadb8b..a9f4f431 100644 --- a/altosuilib/AltosGraphDataPoint.java +++ b/altosuilib/AltosGraphDataPoint.java @@ -40,15 +40,15 @@ public class AltosGraphDataPoint implements AltosUIDataPoint { 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; @@ -128,53 +128,32 @@ public class AltosGraphDataPoint implements AltosUIDataPoint { 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();