X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosState.java;h=ad032a1685af8548af8a3b763f6c8d447328b131;hp=7126315168448d2765b5beb518bcfdf1b141d7e1;hb=85013045ca505096064aaf45c312b158d0263d2a;hpb=f0216d721ed13f4d3dc608bb6ad8f83732b27c0a diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index 71263151..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_4; +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,14 +276,15 @@ public class AltosState implements Cloneable { } AltosCValue() { - measured = new AltosValue(); - computed = new AltosValue(); + measured = new AltosIValue(); + computed = new AltosIValue(); } } public int state; public int flight; public int serial; + public int altitude_32; public int receiver_serial; public boolean landed; public boolean ascent; /* going up? */ @@ -300,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(); @@ -324,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()) @@ -347,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) @@ -369,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(); @@ -389,6 +403,11 @@ public class AltosState implements Cloneable { private AltosGpsAltitude gps_altitude; + private AltosValue gps_ground_speed; + private AltosValue gps_ascent_rate; + private AltosValue gps_course; + private AltosValue gps_speed; + public double altitude() { double a = altitude.value(); if (a != AltosLib.MISSING) @@ -419,7 +438,35 @@ public class AltosState implements Cloneable { gps_altitude.set(new_gps_altitude, time); } - class AltosPressure extends AltosValue { + public double gps_ground_speed() { + return gps_ground_speed.value(); + } + + public double max_gps_ground_speed() { + return gps_ground_speed.max(); + } + + public double gps_ascent_rate() { + return gps_ascent_rate.value(); + } + + public double max_gps_ascent_rate() { + return gps_ascent_rate.max(); + } + + public double gps_course() { + return gps_course.value(); + } + + public double gps_speed() { + return gps_speed.value(); + } + + public double max_gps_speed() { + return gps_speed.max(); + } + + class AltosPressure extends AltosValue implements Serializable { void set(double p, double time) { super.set(p, time); if (state == AltosLib.ao_flight_pad) @@ -439,11 +486,7 @@ public class AltosState implements Cloneable { pressure.set(p, time); } - public double height() { - double k = kalman_height.value(); - if (k != AltosLib.MISSING) - return k; - + public double baro_height() { double a = altitude(); double g = ground_altitude(); if (a != AltosLib.MISSING && g != AltosLib.MISSING) @@ -451,6 +494,18 @@ public class AltosState implements Cloneable { return AltosLib.MISSING; } + public double height() { + double k = kalman_height.value(); + if (k != AltosLib.MISSING) + return k; + + double b = baro_height(); + if (b != AltosLib.MISSING) + return b; + + return gps_height(); + } + public double max_height() { double k = kalman_height.max(); if (k != AltosLib.MISSING) @@ -460,7 +515,7 @@ public class AltosState implements Cloneable { double g = ground_altitude(); if (a != AltosLib.MISSING && g != AltosLib.MISSING) return a - g; - return AltosLib.MISSING; + return max_gps_height(); } public double gps_height() { @@ -481,10 +536,10 @@ 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; + return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless; } void set_accel() { @@ -513,20 +568,32 @@ public class AltosState implements Cloneable { double v = kalman_speed.value(); if (v != AltosLib.MISSING) return v; - return speed.value(); + v = speed.value(); + if (v != AltosLib.MISSING) + return v; + v = gps_speed(); + if (v != AltosLib.MISSING) + return v; + return AltosLib.MISSING; } public double max_speed() { double v = kalman_speed.max(); if (v != AltosLib.MISSING) return v; - return speed.max(); + v = speed.max(); + if (v != AltosLib.MISSING) + return v; + v = max_gps_speed(); + if (v != AltosLib.MISSING) + return v; + return AltosLib.MISSING; } - class AltosAccel extends AltosCValue { + class AltosAccel extends AltosCValue implements Serializable { boolean can_max() { - return state < AltosLib.ao_flight_fast; + return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless; } void set_measured(double a, double time) { @@ -546,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() { @@ -614,6 +681,7 @@ public class AltosState implements Cloneable { public double ground_accel_avg; public int log_format; + public String product; public AltosMs5607 baro; @@ -658,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; @@ -678,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; @@ -694,11 +779,16 @@ public class AltosState implements Cloneable { gps_altitude = new AltosGpsAltitude(); gps_ground_altitude = new AltosGpsGroundAltitude(); + gps_ground_speed = new AltosValue(); + gps_speed = new AltosValue(); + gps_ascent_rate = new AltosValue(); + gps_course = new AltosValue(); speak_tick = AltosLib.MISSING; speak_altitude = AltosLib.MISSING; callsign = null; + firmware_version = null; accel_plus_g = AltosLib.MISSING; accel_minus_g = AltosLib.MISSING; @@ -708,8 +798,10 @@ public class AltosState implements Cloneable { ground_accel_avg = AltosLib.MISSING; log_format = AltosLib.MISSING; + product = null; serial = AltosLib.MISSING; receiver_serial = AltosLib.MISSING; + altitude_32 = AltosLib.MISSING; baro = null; companion = null; @@ -799,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(); @@ -822,6 +935,10 @@ public class AltosState implements Cloneable { gps_altitude.copy(old.gps_altitude); gps_ground_altitude.copy(old.gps_ground_altitude); + gps_ground_speed.copy(old.gps_ground_speed); + gps_ascent_rate.copy(old.gps_ascent_rate); + gps_course.copy(old.gps_course); + gps_speed.copy(old.gps_speed); pad_lat = old.pad_lat; pad_lon = old.pad_lon; @@ -831,6 +948,7 @@ public class AltosState implements Cloneable { speak_altitude = old.speak_altitude; callsign = old.callsign; + firmware_version = old.firmware_version; accel_plus_g = old.accel_plus_g; accel_minus_g = old.accel_minus_g; @@ -839,8 +957,10 @@ public class AltosState implements Cloneable { ground_accel_avg = old.ground_accel_avg; log_format = old.log_format; + product = old.product; serial = old.serial; receiver_serial = old.receiver_serial; + altitude_32 = old.altitude_32; baro = old.baro; companion = old.companion; @@ -852,17 +972,17 @@ public class AltosState implements Cloneable { } void update_gps() { - elevation = 0; - range = -1; + elevation = AltosLib.MISSING; + range = AltosLib.MISSING; if (gps == null) return; if (gps.locked && gps.nsat >= 4) { /* Track consecutive 'good' gps reports, waiting for 10 of them */ - if (state == AltosLib.ao_flight_pad) { + if (state == AltosLib.ao_flight_pad || state == AltosLib.ao_flight_stateless) { set_npad(npad+1); - if (pad_lat != AltosLib.MISSING) { + if (pad_lat != AltosLib.MISSING && (npad < 10 || state == AltosLib.ao_flight_pad)) { pad_lat = (pad_lat * 31 + gps.lat) / 32; pad_lon = (pad_lon * 31 + gps.lon) / 32; gps_ground_altitude.set_filtered(gps.alt, time); @@ -874,6 +994,15 @@ public class AltosState implements Cloneable { gps_ground_altitude.set(gps.alt, time); } gps_altitude.set(gps.alt, time); + if (gps.climb_rate != AltosLib.MISSING) + gps_ascent_rate.set(gps.climb_rate, time); + if (gps.ground_speed != AltosLib.MISSING) + gps_ground_speed.set(gps.ground_speed, time); + if (gps.climb_rate != AltosLib.MISSING && gps.ground_speed != AltosLib.MISSING) + gps_speed.set(Math.sqrt(gps.ground_speed * gps.ground_speed + + gps.climb_rate * gps.climb_rate), time); + if (gps.course != AltosLib.MISSING) + gps_course.set(gps.course, time); } if (gps.lat != 0 && gps.lon != 0 && pad_lat != AltosLib.MISSING && @@ -918,18 +1047,34 @@ public class AltosState implements Cloneable { state <= AltosLib.ao_flight_coast); boost = (AltosLib.ao_flight_boost == state); } - } public void set_device_type(int device_type) { this.device_type = device_type; + switch (device_type) { + case AltosLib.product_telegps: + this.state = AltosLib.ao_flight_stateless; + break; + } } - public void set_config(int major, int minor, int apogee_delay, int main_deploy, int flight_log_max) { - config_major = major; - config_minor = minor; + public void set_log_format(int log_format) { + this.log_format = log_format; + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_TELEGPS: + this.state = AltosLib.ao_flight_stateless; + break; + } + } + + public void set_flight_params(int apogee_delay, int main_deploy) { this.apogee_delay = apogee_delay; this.main_deploy = main_deploy; + } + + public void set_config(int major, int minor, int flight_log_max) { + config_major = major; + config_minor = minor; this.flight_log_max = flight_log_max; } @@ -941,15 +1086,27 @@ public class AltosState implements Cloneable { firmware_version = version; } + public int compare_version(String other_version) { + if (firmware_version == null) + return AltosLib.MISSING; + return AltosLib.compare_version(firmware_version, other_version); + } + + private void re_init() { + int bt = boost_tick; + int rs = receiver_serial; + init(); + boost_tick = bt; + receiver_serial = rs; + } + public void set_flight(int flight) { /* When the flight changes, reset the state */ - if (flight != AltosLib.MISSING && flight != 0) { + if (flight != AltosLib.MISSING) { if (this.flight != AltosLib.MISSING && this.flight != flight) { - int bt = boost_tick; - init(); - boost_tick = bt; + re_init(); } this.flight = flight; } @@ -960,9 +1117,7 @@ public class AltosState implements Cloneable { if (serial != AltosLib.MISSING) { if (this.serial != AltosLib.MISSING && this.serial != serial) { - int bt = boost_tick; - init(); - boost_tick = bt; + re_init(); } this.serial = serial; } @@ -973,6 +1128,15 @@ public class AltosState implements Cloneable { receiver_serial = serial; } + public boolean altitude_32() { + return altitude_32 == 1; + } + + public void set_altitude_32(int altitude_32) { + if (altitude_32 != AltosLib.MISSING) + this.altitude_32 = altitude_32; + } + public int rssi() { if (rssi == AltosLib.MISSING) return 0; @@ -999,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();