X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=altoslib%2FAltosState.java;h=846bda42dd834f454c8dc225de5bc4a42a44643f;hb=e8794ab1d512418e312a1a779777119e20dc8c38;hp=d391dcfb2f2773c90ec4cb711c54aa7d97ea076d;hpb=876acbdc22ff93c22836f789e0b6394eb19e0da3;p=fw%2Faltos diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index d391dcfb..846bda42 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -3,7 +3,8 @@ * * 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. + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. * * This program is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of @@ -19,9 +20,11 @@ * Track flight state from telemetry or eeprom data stream */ -package org.altusmetrum.altoslib_4; +package org.altusmetrum.altoslib_11; -public class AltosState implements Cloneable { +import java.io.*; + +public class AltosState extends AltosFlightListener implements Cloneable { public static final int set_position = 1; public static final int set_gps = 2; @@ -29,8 +32,9 @@ public class AltosState implements Cloneable { public int set; + static final double filter_len = 2.0; static final double ascent_filter_len = 0.5; - static final double descent_filter_len = 0.5; + static final double descent_filter_len = 5.0; /* derived data */ @@ -39,13 +43,11 @@ public class AltosState implements Cloneable { public double time; public double prev_time; public double time_change; - public int tick; private int prev_tick; - 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; @@ -62,8 +64,10 @@ public class AltosState implements Cloneable { } void set_filtered(double new_value, double time) { - if (prev_value != AltosLib.MISSING) - new_value = (prev_value * 15.0 + new_value) / 16.0; + if (prev_value != AltosLib.MISSING) { + double f = 1/Math.exp((time - prev_set_time) / filter_len); + new_value = f * new_value + (1-f) * prev_value; + } set(new_value, time); } @@ -177,11 +181,27 @@ public class AltosState implements Cloneable { prev_value = AltosLib.MISSING; max_value = AltosLib.MISSING; } + } class AltosCValue { - AltosValue measured; - AltosValue computed; + + class AltosIValue extends AltosValue { + boolean can_max() { + return c_can_max(); + } + + AltosIValue() { + super(); + } + }; + + public AltosIValue measured; + public AltosIValue computed; + + boolean can_max() { return true; } + + boolean c_can_max() { return can_max(); } double value() { double v = measured.value(); @@ -262,15 +282,16 @@ public class AltosState implements Cloneable { computed.finish_update(); } - AltosCValue() { - measured = new AltosValue(); - computed = new AltosValue(); + public AltosCValue() { + measured = new AltosIValue(); + computed = new AltosIValue(); } } - public int state; + private int state; public int flight; public int serial; + public int altitude_32; public int receiver_serial; public boolean landed; public boolean ascent; /* going up? */ @@ -312,6 +333,10 @@ public class AltosState implements Cloneable { pad_alt = value(); gps_altitude.set_gps_height(); } + + AltosGpsGroundAltitude() { + super(); + } } private AltosGpsGroundAltitude gps_ground_altitude; @@ -335,6 +360,10 @@ public class AltosState implements Cloneable { super.set_measured(p, time); ground_altitude.set_computed(pressure_to_altitude(p), time); } + + AltosGroundPressure () { + super(); + } } private AltosGroundPressure ground_pressure; @@ -365,6 +394,10 @@ public class AltosState implements Cloneable { set_speed(measured); set |= set_position; } + + AltosAltitude() { + super(); + } } private AltosAltitude altitude; @@ -385,6 +418,10 @@ public class AltosState implements Cloneable { super.set(a, t); set_gps_height(); } + + AltosGpsAltitude() { + super(); + } } private AltosGpsAltitude gps_altitude; @@ -460,6 +497,10 @@ public class AltosState implements Cloneable { double a = pressure_to_altitude(p); altitude.set_computed(a, time); } + + AltosPressure() { + super(); + } } private AltosPressure pressure; @@ -472,15 +513,42 @@ public class AltosState implements Cloneable { pressure.set(p, time); } - public double height() { - double k = kalman_height.value(); - if (k != AltosLib.MISSING) - return k; + class AltosForce extends AltosValue { + void set(double p, double time) { + super.set(p, time); + } + AltosForce() { + super(); + } + } + private AltosForce thrust; + + public double thrust() { + return thrust.value(); + } + + public void set_thrust(double N) { + thrust.set(N, time); + } + + public double baro_height() { double a = altitude(); double g = ground_altitude(); if (a != AltosLib.MISSING && g != AltosLib.MISSING) return a - g; + 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(); } @@ -538,6 +606,10 @@ public class AltosState implements Cloneable { super.set_measured(new_value, time); set_accel(); } + + AltosSpeed() { + super(); + } } private AltosSpeed speed; @@ -579,6 +651,10 @@ public class AltosState implements Cloneable { if (ascent) speed.set_integral(this.measured); } + + AltosAccel() { + super(); + } } AltosAccel acceleration; @@ -591,10 +667,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() { @@ -659,6 +735,7 @@ public class AltosState implements Cloneable { public double ground_accel_avg; public int log_format; + public int log_space; public String product; public AltosMs5607 baro; @@ -702,9 +779,10 @@ public class AltosState implements Cloneable { ground_pressure = new AltosGroundPressure(); altitude = new AltosAltitude(); pressure = new AltosPressure(); + thrust = new AltosForce(); speed = new AltosSpeed(); acceleration = new AltosAccel(); - orient = new AltosValue(); + orient = new AltosCValue(); temperature = AltosLib.MISSING; battery_voltage = AltosLib.MISSING; @@ -724,7 +802,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; @@ -759,9 +854,11 @@ public class AltosState implements Cloneable { ground_accel_avg = AltosLib.MISSING; log_format = AltosLib.MISSING; + log_space = AltosLib.MISSING; product = null; serial = AltosLib.MISSING; receiver_serial = AltosLib.MISSING; + altitude_32 = AltosLib.MISSING; baro = null; companion = null; @@ -791,6 +888,8 @@ public class AltosState implements Cloneable { return; } + super.copy(old); + received_time = old.received_time; time = old.time; time_change = old.time_change; @@ -851,6 +950,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(); @@ -896,9 +1016,11 @@ public class AltosState implements Cloneable { ground_accel_avg = old.ground_accel_avg; log_format = old.log_format; + log_space = old.log_space; product = old.product; serial = old.serial; receiver_serial = old.receiver_serial; + altitude_32 = old.altitude_32; baro = old.baro; companion = old.companion; @@ -910,8 +1032,8 @@ public class AltosState implements Cloneable { } void update_gps() { - elevation = 0; - range = -1; + elevation = AltosLib.MISSING; + range = AltosLib.MISSING; if (gps == null) return; @@ -969,15 +1091,14 @@ public class AltosState implements Cloneable { } } - public void set_boost_tick(int boost_tick) { - if (boost_tick != AltosLib.MISSING) - this.boost_tick = boost_tick; - } - public String state_name() { return AltosLib.state_name(state); } + public void set_product(String product) { + this.product = product; + } + public void set_state(int state) { if (state != AltosLib.ao_flight_invalid) { this.state = state; @@ -987,6 +1108,10 @@ public class AltosState implements Cloneable { } } + public int state() { + return state; + } + public void set_device_type(int device_type) { this.device_type = device_type; switch (device_type) { @@ -1005,6 +1130,10 @@ public class AltosState implements Cloneable { } } + public void set_log_space(int log_space) { + this.log_space = log_space; + } + public void set_flight_params(int apogee_delay, int main_deploy) { this.apogee_delay = apogee_delay; this.main_deploy = main_deploy; @@ -1024,15 +1153,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; } @@ -1043,9 +1184,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; } @@ -1056,6 +1195,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; @@ -1082,16 +1230,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(); @@ -1101,7 +1403,7 @@ public class AltosState implements Cloneable { public void set_ms5607(AltosMs5607 ms5607) { baro = ms5607; - if (baro != null) { + if (baro != null && baro.pa != AltosLib.MISSING && baro.cc != AltosLib.MISSING) { set_pressure(baro.pa); set_temperature(baro.cc / 100.0); } @@ -1116,11 +1418,6 @@ public class AltosState implements Cloneable { } } - public void make_companion (int nchannels) { - if (companion == null) - companion = new AltosCompanion(nchannels); - } - public void set_companion(AltosCompanion companion) { this.companion = companion; } @@ -1220,28 +1517,57 @@ public class AltosState implements Cloneable { return tick != AltosLib.MISSING && serial != AltosLib.MISSING; } - public AltosGPS make_temp_gps(boolean sats) { - if (temp_gps == null) { - temp_gps = new AltosGPS(gps); - } - gps_pending = true; - if (sats) { - if (tick != temp_gps_sat_tick) - temp_gps.cc_gps_sat = null; - temp_gps_sat_tick = tick; - } - return temp_gps; - } - public void set_temp_gps() { set_gps(temp_gps, gps_sequence + 1); gps_pending = false; - temp_gps = null; + super.set_temp_gps(); + } + + public void set_config_data(AltosConfigData config_data) { + if (config_data.callsign != null) + set_callsign(config_data.callsign); + if (config_data.accel_cal_plus != AltosLib.MISSING && + config_data.accel_cal_minus != AltosLib.MISSING) + set_accel_g(config_data.accel_cal_plus, config_data.accel_cal_minus); + if (config_data.product != null) + set_product(config_data.product); + if (config_data.log_format != AltosLib.MISSING) + set_log_format(config_data.log_format); + if (config_data.serial != AltosLib.MISSING) + set_serial(config_data.serial); + AltosMs5607 ms5607 = new AltosMs5607(config_data); + if (ms5607.valid_config()) + set_ms5607(ms5607); } public AltosState clone() { AltosState s = new AltosState(); s.copy(this); + + /* Code to test state save/restore. Enable only for that purpose + */ + if (false) { + AltosJson json = new AltosJson(this); + String onetrip = json.toPrettyString(); + AltosJson back = AltosJson.fromString(onetrip); + AltosState tripstate = (AltosState) back.make(this.getClass()); + AltosJson tripjson = new AltosJson(tripstate); + String twotrip = tripjson.toPrettyString(); + + if (!onetrip.equals(twotrip)) { + try { + FileWriter one_file = new FileWriter("one.json", true); + one_file.write(onetrip); + one_file.flush(); + FileWriter two_file = new FileWriter("two.json", true); + two_file.write(twotrip); + two_file.flush(); + } catch (Exception e) { + } + System.out.printf("json error\n"); + System.exit(1); + } + } return s; }