summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--altoslib/AltosCalData.java26
-rw-r--r--altoslib/AltosConvert.java13
-rw-r--r--altoslib/AltosEepromRecordMega.java8
-rw-r--r--altoslib/AltosFlightSeries.java58
-rw-r--r--altoslib/AltosIMU.java8
-rw-r--r--altoslib/AltosState.java33
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();
}
generated by cgit v1.2.3 (git 2.39.1) at 2025-10-31 22:58:27 +0000