X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosState.java;h=ad032a1685af8548af8a3b763f6c8d447328b131;hp=5fce15c43865e63b43447db93f59faf725f838f7;hb=85013045ca505096064aaf45c312b158d0263d2a;hpb=4828be0ca5252ac9cd6061209385dcd6c4c57965 diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index 5fce15c4..ad032a16 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -19,9 +19,11 @@ * Track flight state from telemetry or eeprom data stream */ -package org.altusmetrum.altoslib_5; +package org.altusmetrum.altoslib_7; -public class AltosState implements Cloneable { +import java.io.*; + +public class AltosState implements Cloneable, Serializable { public static final int set_position = 1; public static final int set_gps = 2; @@ -43,9 +45,9 @@ public class AltosState implements Cloneable { private int prev_tick; public int boost_tick; - class AltosValue { - private double value; - private double prev_value; + class AltosValue implements Serializable{ + double value; + double prev_value; private double max_value; private double set_time; private double prev_set_time; @@ -179,9 +181,20 @@ public class AltosState implements Cloneable { } } - class AltosCValue { - AltosValue measured; - AltosValue computed; + class AltosCValue implements Serializable { + + class AltosIValue extends AltosValue implements Serializable { + 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 +276,8 @@ public class AltosState implements Cloneable { } AltosCValue() { - measured = new AltosValue(); - computed = new AltosValue(); + measured = new AltosIValue(); + computed = new AltosIValue(); } } @@ -301,7 +314,7 @@ public class AltosState implements Cloneable { ground_altitude.set_measured(a, time); } - class AltosGpsGroundAltitude extends AltosValue { + class AltosGpsGroundAltitude extends AltosValue implements Serializable { void set(double a, double t) { super.set(a, t); pad_alt = value(); @@ -325,7 +338,7 @@ public class AltosState implements Cloneable { gps_ground_altitude.set(a, time); } - class AltosGroundPressure extends AltosCValue { + class AltosGroundPressure extends AltosCValue implements Serializable { void set_filtered(double p, double time) { computed.set_filtered(p, time); if (!is_measured()) @@ -348,7 +361,7 @@ public class AltosState implements Cloneable { ground_pressure.set_measured(pressure, time); } - class AltosAltitude extends AltosCValue { + class AltosAltitude extends AltosCValue implements Serializable { private void set_speed(AltosValue v) { if (!acceleration.is_measured() || !ascent) @@ -370,7 +383,7 @@ public class AltosState implements Cloneable { private AltosAltitude altitude; - class AltosGpsAltitude extends AltosValue { + class AltosGpsAltitude extends AltosValue implements Serializable { private void set_gps_height() { double a = value(); @@ -453,7 +466,7 @@ public class AltosState implements Cloneable { return gps_speed.max(); } - class AltosPressure extends AltosValue { + class AltosPressure extends AltosValue implements Serializable { void set(double p, double time) { super.set(p, time); if (state == AltosLib.ao_flight_pad) @@ -523,7 +536,7 @@ public class AltosState implements Cloneable { return AltosLib.MISSING; } - class AltosSpeed extends AltosCValue { + class AltosSpeed extends AltosCValue implements Serializable { boolean can_max() { return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless; @@ -577,7 +590,7 @@ public class AltosState implements Cloneable { return AltosLib.MISSING; } - class AltosAccel extends AltosCValue { + class AltosAccel extends AltosCValue implements Serializable { boolean can_max() { return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless; @@ -600,10 +613,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 +726,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 +746,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 +891,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 +1163,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();