From 32de85691f2e4ed1430a259e05a514ad820b32d9 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 8 Jun 2017 20:37:58 -0700 Subject: [PATCH] altoslib: Compute orientation from eeprom data files This was lost in the AltosFlightSeries transformation. Signed-off-by: Keith Packard --- altoslib/AltosCalData.java | 26 ++++++++++--- altoslib/AltosConvert.java | 13 +++++++ altoslib/AltosEepromRecordMega.java | 8 ++-- altoslib/AltosFlightSeries.java | 58 +++++++++++++++++++++++++++-- altoslib/AltosIMU.java | 8 ++-- altoslib/AltosState.java | 33 ++++------------ 6 files changed, 105 insertions(+), 41 deletions(-) diff --git a/altoslib/AltosCalData.java b/altoslib/AltosCalData.java index 2eff6ac1..49b90ce7 100644 --- a/altoslib/AltosCalData.java +++ b/altoslib/AltosCalData.java @@ -313,25 +313,27 @@ public class AltosCalData { gyro_zero_roll = roll; gyro_zero_pitch = pitch; gyro_zero_yaw = yaw; + imu_wrap_checked = false; } } public double gyro_roll(double counts) { if (gyro_zero_roll == AltosLib.MISSING || counts == AltosLib.MISSING) return AltosLib.MISSING; - return AltosIMU.convert_gyro(counts - gyro_zero_roll); + + return AltosIMU.gyro_degrees_per_second(counts, gyro_zero_roll); } public double gyro_pitch(double counts) { if (gyro_zero_pitch == AltosLib.MISSING || counts == AltosLib.MISSING) return AltosLib.MISSING; - return AltosIMU.convert_gyro(counts - gyro_zero_pitch); + return AltosIMU.gyro_degrees_per_second(counts, gyro_zero_pitch); } public double gyro_yaw(double counts) { if (gyro_zero_yaw == AltosLib.MISSING || counts == AltosLib.MISSING) return AltosLib.MISSING; - return AltosIMU.convert_gyro(counts - gyro_zero_yaw); + return AltosIMU.gyro_degrees_per_second(counts, gyro_zero_yaw); } private double gyro_zero_overflow(double first) { @@ -340,13 +342,25 @@ public class AltosCalData { v = Math.ceil(v); else v = Math.floor(v); + if (v != 0) + System.out.printf("Adjusting gyro axis by %g steps\n", v); return v * 128.0; } + /* Initial TeleMega log format had only 16 bits for gyro cal, so the top 9 bits got lost as the + * cal data are scaled by 512. Use the first sample to adjust the cal value, assuming that it is + * from a time of fairly low rotation speed. Fixed in later TeleMega firmware by storing 32 bits + * of cal values. + */ + private boolean imu_wrap_checked = false; + public void check_imu_wrap(double roll, double pitch, double yaw) { - gyro_zero_roll += gyro_zero_overflow(roll); - gyro_zero_pitch += gyro_zero_overflow(pitch); - gyro_zero_yaw += gyro_zero_overflow(yaw); + if (!imu_wrap_checked) { + gyro_zero_roll += gyro_zero_overflow(roll); + gyro_zero_pitch += gyro_zero_overflow(pitch); + gyro_zero_yaw += gyro_zero_overflow(yaw); + imu_wrap_checked = true; + } } public double mag_along(double along) { diff --git a/altoslib/AltosConvert.java b/altoslib/AltosConvert.java index a252abdf..0d25c6d7 100644 --- a/altoslib/AltosConvert.java +++ b/altoslib/AltosConvert.java @@ -184,6 +184,18 @@ public class AltosConvert { return altitude; } + public static double degrees_to_radians(double degrees) { + if (degrees == AltosLib.MISSING) + return AltosLib.MISSING; + return degrees * (Math.PI / 180.0); + } + + public static double radians_to_degrees(double radians) { + if (radians == AltosLib.MISSING) + return AltosLib.MISSING; + return radians * (180.0 / Math.PI); + } + public static double cc_battery_to_voltage(double battery) { @@ -392,6 +404,7 @@ public class AltosConvert { } public static double acceleration_from_sensor(double sensor, double plus_g, double minus_g, double ground) { + if (sensor == AltosLib.MISSING) return AltosLib.MISSING; diff --git a/altoslib/AltosEepromRecordMega.java b/altoslib/AltosEepromRecordMega.java index 0abc3fe7..18d435af 100644 --- a/altoslib/AltosEepromRecordMega.java +++ b/altoslib/AltosEepromRecordMega.java @@ -161,11 +161,13 @@ public class AltosEepromRecordMega extends AltosEepromRecord { cal_data.mag_through(mag_through)); + final double lsb_per_g = 1920.0/105.5; + double acceleration = AltosConvert.acceleration_from_sensor( accel(), - config_data.accel_cal_plus, - config_data.accel_cal_minus, - AltosLib.MISSING); + cal_data.ground_accel, + cal_data.ground_accel + 2 * lsb_per_g, + cal_data.ground_accel); listener.set_acceleration(acceleration); break; diff --git a/altoslib/AltosFlightSeries.java b/altoslib/AltosFlightSeries.java index 0b60fdf5..dad066d7 100644 --- a/altoslib/AltosFlightSeries.java +++ b/altoslib/AltosFlightSeries.java @@ -304,6 +304,49 @@ public class AltosFlightSeries extends AltosDataListener { add_series(speed_series); } + public AltosTimeSeries orient_series; + + public static final String orient_name = "Tilt Angle"; + + private void compute_orient() { + + if (orient_series != null) + return; + + if (accel_ground_across == AltosLib.MISSING) + return; + + if (cal_data.pad_orientation == AltosLib.MISSING) + return; + + if (cal_data.accel_zero_across == AltosLib.MISSING) + return; + + AltosRotation rotation = new AltosRotation(AltosIMU.convert_accel(accel_ground_across - cal_data.accel_zero_across), + AltosIMU.convert_accel(accel_ground_through - cal_data.accel_zero_through), + AltosIMU.convert_accel(accel_ground_along - cal_data.accel_zero_along), + cal_data.pad_orientation); + double prev_time = ground_time; + + orient_series = add_series(orient_name, AltosConvert.orient); + orient_series.add(ground_time, rotation.tilt()); + + for (AltosTimeValue roll_v : gyro_roll) { + double time = roll_v.time; + double dt = time - prev_time; + + if (dt > 0) { + double roll = AltosConvert.degrees_to_radians(roll_v.value); + double pitch = AltosConvert.degrees_to_radians(gyro_pitch.value(time)); + double yaw = AltosConvert.degrees_to_radians(gyro_yaw.value(time)); + + rotation.rotate(dt, pitch, yaw, roll); + orient_series.add(time, rotation.tilt()); + } + prev_time = time; + } + } + public AltosTimeSeries kalman_height_series, kalman_speed_series, kalman_accel_series; public static final String kalman_height_name = "Kalman Height"; @@ -499,7 +542,17 @@ public class AltosFlightSeries extends AltosDataListener { accel_through.add(time(), through); } + private double accel_ground_along = AltosLib.MISSING; + private double accel_ground_across = AltosLib.MISSING; + private double accel_ground_through = AltosLib.MISSING; + + private double ground_time; + public void set_accel_ground(double along, double across, double through) { + accel_ground_along = along; + accel_ground_across = across; + accel_ground_through = through; + ground_time = time(); } public void set_gyro(double roll, double pitch, double yaw) { @@ -524,10 +577,6 @@ public class AltosFlightSeries extends AltosDataListener { mag_through.add(time(), through); } - public static final String orient_name = "Tilt Angle"; - - public AltosTimeSeries orient_series; - public void set_orient(double orient) { if (orient_series == null) orient_series = add_series(orient_name, AltosConvert.orient); @@ -604,6 +653,7 @@ public class AltosFlightSeries extends AltosDataListener { } public void finish() { + compute_orient(); compute_speed(); compute_accel(); compute_height(); diff --git a/altoslib/AltosIMU.java b/altoslib/AltosIMU.java index 20a2a413..b434e02e 100644 --- a/altoslib/AltosIMU.java +++ b/altoslib/AltosIMU.java @@ -36,10 +36,12 @@ public class AltosIMU implements Cloneable { return counts / counts_per_g * AltosConvert.gravity; } - public static final double counts_per_degsec = 16.4; + /* In radians */ + public static final double GYRO_FULLSCALE_DEGREES = 2000.0; + public static final double GYRO_COUNTS = 32767.0; - public static double convert_gyro(double counts) { - return counts / counts_per_degsec; + public static double gyro_degrees_per_second(double counts, double cal) { + return (counts - cal) * GYRO_FULLSCALE_DEGREES / GYRO_COUNTS; } public boolean parse_string(String line) { diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index 5abc0c2d..caa1cb4a 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -22,8 +22,6 @@ package org.altusmetrum.altoslib_11; -import java.io.*; - public class AltosState extends AltosDataListener { public static final int set_position = 1; @@ -759,8 +757,6 @@ public class AltosState extends AltosDataListener { accel_ground_across = AltosLib.MISSING; accel_ground_through = AltosLib.MISSING; - pad_orientation = AltosLib.MISSING; - set_npad(0); ngps = 0; @@ -903,16 +899,14 @@ public class AltosState extends AltosDataListener { public AltosRotation rotation; public AltosRotation ground_rotation; - 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) { + if (cal_data.pad_orientation != AltosLib.MISSING && accel_ground_along != AltosLib.MISSING) { rotation = new AltosRotation(AltosIMU.convert_accel(accel_ground_across - cal_data.accel_zero_across), AltosIMU.convert_accel(accel_ground_through - cal_data.accel_zero_through), AltosIMU.convert_accel(accel_ground_along - cal_data.accel_zero_along), - pad_orientation); + cal_data.pad_orientation); ground_rotation = rotation; orient.set_computed(rotation.tilt(), time); } @@ -925,28 +919,17 @@ public class AltosState extends AltosDataListener { update_pad_rotation(); } - public void set_pad_orientation(int pad_orientation) { - this.pad_orientation = pad_orientation; - update_pad_rotation(); - } - 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()); + double pitch = AltosConvert.degrees_to_radians(gyro_pitch()); + double yaw = AltosConvert.degrees_to_radians(gyro_yaw()); + double roll = AltosConvert.degrees_to_radians(gyro_roll()); - if (t > 0 & pitch != AltosLib.MISSING && rotation != null) { + if (t > 0 && pitch != AltosLib.MISSING && rotation != null) { rotation.rotate(t, pitch, yaw, roll); orient.set_computed(rotation.tilt(), time); } @@ -958,8 +941,8 @@ public class AltosState extends AltosDataListener { public void set_gyro(double roll, double pitch, double yaw) { gyro_roll = roll; - gyro_pitch = roll; - gyro_roll = roll; + gyro_pitch = pitch; + gyro_roll = yaw; update_orient(); } -- 2.30.2