X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosState.java;h=aa3de432a75fa2e7feaaf9522d967ba1555213ed;hp=b40b744fd7b5ae13894095aa2bf278758c309d62;hb=017ed54ff69ef2f7740ea2578e22bf72e88deafb;hpb=04d7d0f829ba953ffeca8ad9887a4b6b2b5d5087 diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index b40b744f..aa3de432 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -22,22 +22,35 @@ package org.altusmetrum.altoslib_1; public class AltosState implements Cloneable { - public AltosRecord data; + public AltosRecord record; + + public static final int set_position = 1; + public static final int set_gps = 2; + public static final int set_data = 4; + + public int set; /* derived data */ public long report_time; public double time; + public double prev_time; public double time_change; public int tick; + public int boost_tick; public int state; + public int flight; + public int serial; public boolean landed; public boolean ascent; /* going up? */ public boolean boost; /* under power */ + public int rssi; + public int status; public double ground_altitude; + public double ground_pressure; public double altitude; public double height; public double pressure; @@ -60,6 +73,8 @@ public class AltosState implements Cloneable { public double kalman_height, kalman_speed, kalman_acceleration; public AltosGPS gps; + public AltosGPS temp_gps; + public boolean gps_pending; public int gps_sequence; public AltosIMU imu; @@ -89,12 +104,14 @@ public class AltosState implements Cloneable { public double accel_minus_g; public double accel; public double ground_accel; + public double ground_accel_avg; public int log_format; - public int serial; public AltosMs5607 baro; + public AltosRecordCompanion companion; + public double speed() { return speed; } @@ -112,17 +129,25 @@ public class AltosState implements Cloneable { } public void init() { - data = new AltosRecord(); + record = null; + + set = 0; report_time = System.currentTimeMillis(); time = AltosRecord.MISSING; time_change = AltosRecord.MISSING; + prev_time = AltosRecord.MISSING; tick = AltosRecord.MISSING; + boost_tick = AltosRecord.MISSING; state = AltosLib.ao_flight_invalid; + flight = AltosRecord.MISSING; landed = false; boost = false; + rssi = AltosRecord.MISSING; + status = 0; ground_altitude = AltosRecord.MISSING; + ground_pressure = AltosRecord.MISSING; altitude = AltosRecord.MISSING; height = AltosRecord.MISSING; pressure = AltosRecord.MISSING; @@ -138,21 +163,20 @@ public class AltosState implements Cloneable { apogee_voltage = AltosRecord.MISSING; main_voltage = AltosRecord.MISSING; - - accel_speed = AltosRecord.MISSING; - baro_speed = AltosRecord.MISSING; + speed = AltosRecord.MISSING; kalman_height = AltosRecord.MISSING; kalman_speed = AltosRecord.MISSING; kalman_acceleration = AltosRecord.MISSING; - max_baro_speed = 0; - max_accel_speed = 0; + max_speed = 0; max_height = 0; max_acceleration = 0; gps = null; + temp_gps = null; gps_sequence = 0; + gps_pending = false; imu = null; mag = null; @@ -165,7 +189,7 @@ public class AltosState implements Cloneable { range = AltosRecord.MISSING; gps_height = AltosRecord.MISSING; - pat_lat = AltosRecord.MISSING; + pad_lat = AltosRecord.MISSING; pad_lon = AltosRecord.MISSING; pad_alt = AltosRecord.MISSING; @@ -176,15 +200,19 @@ public class AltosState implements Cloneable { accel_plus_g = AltosRecord.MISSING; accel_minus_g = AltosRecord.MISSING; + accel = AltosRecord.MISSING; + ground_accel = AltosRecord.MISSING; + ground_accel_avg = AltosRecord.MISSING; log_format = AltosRecord.MISSING; serial = AltosRecord.MISSING; baro = null; + companion = null; } void copy(AltosState old) { - data = null; + record = null; if (old == null) { init(); @@ -193,13 +221,19 @@ public class AltosState implements Cloneable { report_time = old.report_time; time = old.time; - time_change = old.time_change; + time_change = 0; tick = old.tick; + boost_tick = old.boost_tick; state = old.state; + flight = old.flight; landed = old.landed; ascent = old.ascent; boost = old.boost; + rssi = old.rssi; + status = old.status; + + set = 0; ground_altitude = old.ground_altitude; altitude = old.altitude; @@ -211,17 +245,16 @@ public class AltosState implements Cloneable { temperature = old.temperature; apogee_voltage = old.apogee_voltage; main_voltage = old.main_voltage; - accel_speed = old.accel_speed; - baro_speed = old.baro_speed; + speed = old.speed; prev_height = old.height; prev_speed = old.speed; prev_acceleration = old.acceleration; + prev_time = old.time; max_height = old.max_height; max_acceleration = old.max_acceleration; - max_accel_speed = old.max_accel_speed; - max_baro_speed = old.max_baro_speed; + max_speed = old.max_speed; kalman_height = old.kalman_height; kalman_speed = old.kalman_speed; @@ -231,7 +264,12 @@ public class AltosState implements Cloneable { gps = old.gps.clone(); else gps = null; + if (old.temp_gps != null) + temp_gps = old.temp_gps.clone(); + else + temp_gps = null; gps_sequence = old.gps_sequence; + gps_pending = old.gps_pending; if (old.imu != null) imu = old.imu.clone(); @@ -268,14 +306,15 @@ public class AltosState implements Cloneable { accel_plus_g = old.accel_plus_g; accel_minus_g = old.accel_minus_g; + accel = old.accel; + ground_accel = old.ground_accel; + ground_accel_avg = old.ground_accel_avg; + log_format = old.log_format; serial = old.serial; baro = old.baro; - } - - double ground_altitude() { - + companion = old.companion; } double altitude() { @@ -289,8 +328,12 @@ public class AltosState implements Cloneable { void update_vertical_pos() { double alt = altitude(); - if (state == AltosLib.ao_flight_pad) { - + + if (state == AltosLib.ao_flight_pad && alt != AltosRecord.MISSING && ground_pressure == AltosRecord.MISSING) { + if (ground_altitude == AltosRecord.MISSING) + ground_altitude = alt; + else + ground_altitude = (ground_altitude * 7 + alt) / 8; } if (kalman_height != AltosRecord.MISSING) @@ -300,11 +343,14 @@ public class AltosState implements Cloneable { else height = AltosRecord.MISSING; + if (height != AltosRecord.MISSING && height > max_height) + max_height = height; + update_speed(); } double motion_filter_value() { - return 1/ Math.exp(time_change/10.0); + return 1/ Math.exp(time_change/2.0); } void update_speed() { @@ -348,12 +394,18 @@ public class AltosState implements Cloneable { } } } + if (boost && speed != AltosRecord.MISSING && speed > max_speed) + max_speed = speed; } void update_accel() { + double ground = ground_accel; + + if (ground == AltosRecord.MISSING) + ground = ground_accel_avg; if (accel == AltosRecord.MISSING) return; - if (ground_Accel == AltosRecord.MISSING) + if (ground == AltosRecord.MISSING) return; if (accel_plus_g == AltosRecord.MISSING) return; @@ -363,10 +415,61 @@ public class AltosState implements Cloneable { double counts_per_g = (accel_minus_g - accel_plus_g) / 2.0; double counts_per_mss = counts_per_g / 9.80665; - acceleration = (ground_accel - accel) / counts_per_mss; + acceleration = (ground - accel) / counts_per_mss; + + /* Only look at accelerometer data under boost */ + if (boost && acceleration != AltosRecord.MISSING && (max_acceleration == AltosRecord.MISSING || acceleration > max_acceleration)) + max_acceleration = acceleration; update_speed(); } + void update_time() { + if (tick != AltosRecord.MISSING) { + time = tick / 100.0; + if (prev_time != AltosRecord.MISSING) + time_change = time - prev_time; + } + } + + void update_gps() { + elevation = 0; + range = -1; + gps_height = 0; + + 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) { + set_npad(npad+1); + if (pad_lat != AltosRecord.MISSING) { + pad_lat = (pad_lat * 31 + gps.lat) / 32; + pad_lon = (pad_lon * 31 + gps.lon) / 32; + pad_alt = (pad_alt * 31 + gps.alt) / 32; + } + } + if (pad_lat == AltosRecord.MISSING) { + pad_lat = gps.lat; + pad_lon = gps.lon; + pad_alt = gps.alt; + } + } + if (gps.lat != 0 && gps.lon != 0 && + pad_lat != AltosRecord.MISSING && + pad_lon != AltosRecord.MISSING) + { + double h = height; + + if (h == AltosRecord.MISSING) + h = 0; + from_pad = new AltosGreatCircle(pad_lat, pad_lon, 0, gps.lat, gps.lon, h); + elevation = from_pad.elevation; + range = from_pad.range; + gps_height = gps.alt - pad_alt; + } + } + public void set_tick(int tick) { if (tick != AltosRecord.MISSING) { if (this.tick != AltosRecord.MISSING) { @@ -380,6 +483,15 @@ public class AltosState implements Cloneable { } } + public void set_boost_tick(int boost_tick) { + if (boost_tick != AltosRecord.MISSING) + this.boost_tick = boost_tick; + } + + public String state_name() { + return AltosLib.state_name(state); + } + public void set_state(int state) { if (state != AltosLib.ao_flight_invalid) { this.state = state; @@ -390,10 +502,47 @@ public class AltosState implements Cloneable { } + public void set_flight(int flight) { + + /* When the flight changes, reset the state */ + if (flight != AltosRecord.MISSING) { + if (this.flight != AltosRecord.MISSING && + this.flight != flight) { + init(); + } + this.flight = flight; + } + } + + public void set_serial(int serial) { + /* When the serial changes, reset the state */ + if (serial != AltosRecord.MISSING) { + if (this.serial != AltosRecord.MISSING && + this.serial != serial) { + init(); + } + this.serial = serial; + } + } + + public int rssi() { + if (rssi == AltosRecord.MISSING) + return 0; + return rssi; + } + + public void set_rssi(int rssi, int status) { + if (rssi != AltosRecord.MISSING) { + this.rssi = rssi; + this.status = status; + } + } + public void set_altitude(double altitude) { if (altitude != AltosRecord.MISSING) { this.altitude = altitude; update_vertical_pos(); + set |= set_position; } } @@ -404,11 +553,21 @@ public class AltosState implements Cloneable { } } + public void set_ground_pressure (double pressure) { + if (pressure != AltosRecord.MISSING) { + this.ground_pressure = pressure; + set_ground_altitude(AltosConvert.pressure_to_altitude(pressure)); + update_vertical_pos(); + } + } + public void set_gps(AltosGPS gps, int sequence) { if (gps != null) { this.gps = gps.clone(); gps_sequence = sequence; + update_gps(); update_vertical_pos(); + set |= set_gps; } } @@ -417,7 +576,6 @@ public class AltosState implements Cloneable { kalman_height = height; kalman_speed = speed; kalman_acceleration = acceleration; - baro_speed = accel_speed = speed; update_vertical_pos(); } } @@ -429,6 +587,29 @@ public class AltosState implements Cloneable { } } + public void make_baro() { + if (baro == null) + baro = new AltosMs5607(); + } + + public void set_ms5607(int pres, int temp) { + if (baro != null) { + baro.set(pres, temp); + + set_pressure(baro.pa); + set_temperature(baro.cc / 100.0); + } + } + + public void make_companion (int nchannels) { + if (companion == null) + companion = new AltosRecordCompanion(nchannels); + } + + public void set_companion(AltosRecordCompanion companion) { + this.companion = companion; + } + public void set_accel_g(double accel_plus_g, double accel_minus_g) { if (accel_plus_g != AltosRecord.MISSING) { this.accel_plus_g = accel_plus_g; @@ -446,41 +627,88 @@ public class AltosState implements Cloneable { public void set_accel(double accel) { if (accel != AltosRecord.MISSING) { this.accel = accel; + if (state == AltosLib.ao_flight_pad) { + if (ground_accel_avg == AltosRecord.MISSING) + ground_accel_avg = accel; + else + ground_accel_avg = (ground_accel_avg * 7 + accel) / 8; + } } update_accel(); } public void set_temperature(double temperature) { - if (temperature != AltosRecord.MISSING) + if (temperature != AltosRecord.MISSING) { this.temperature = temperature; + set |= set_data; + } } public void set_battery_voltage(double battery_voltage) { - if (battery_voltage != AltosRecord.MISSING) + if (battery_voltage != AltosRecord.MISSING) { this.battery_voltage = battery_voltage; + set |= set_data; + } } public void set_pyro_voltage(double pyro_voltage) { - if (pyro_voltage != AltosRecord.MISSING) + if (pyro_voltage != AltosRecord.MISSING) { this.pyro_voltage = pyro_voltage; + set |= set_data; + } } public void set_apogee_voltage(double apogee_voltage) { - if (apogee_voltage != AltosRecord.MISSING) + if (apogee_voltage != AltosRecord.MISSING) { this.apogee_voltage = apogee_voltage; + set |= set_data; + } } public void set_main_voltage(double main_voltage) { - if (main_voltage != AltosRecord.MISSING) + if (main_voltage != AltosRecord.MISSING) { this.main_voltage = main_voltage; + set |= set_data; + } + } + + + public double time_since_boost() { + if (tick == AltosRecord.MISSING) + return 0.0; + + if (boost_tick != AltosRecord.MISSING) { + return (tick - boost_tick) / 100.0; + } + return tick / 100.0; + } + + public boolean valid() { + return tick != AltosRecord.MISSING && serial != AltosRecord.MISSING; + } + + public AltosGPS make_temp_gps() { + if (temp_gps == null) { + temp_gps = new AltosGPS(gps); + temp_gps.cc_gps_sat = null; + } + gps_pending = true; + return temp_gps; + } + + public void set_temp_gps() { + set_gps(temp_gps, gps_sequence + 1); + gps_pending = false; + temp_gps = null; } public void init (AltosRecord cur, AltosState prev_state) { + System.out.printf ("init\n"); if (cur == null) cur = new AltosRecord(); - data = cur; + record = cur; /* Discard previous state if it was for a different board */ if (prev_state != null && prev_state.serial != cur.serial) @@ -488,146 +716,35 @@ public class AltosState implements Cloneable { copy(prev_state); - set_ground_altitude(data.ground_altitude()); - set_altitude(data.altitude()); + set_ground_altitude(cur.ground_altitude()); + set_altitude(cur.altitude()); - set_kalman(data.kalman_height, data.kalman_speed, data.kalman_acceleration); + set_kalman(cur.kalman_height, cur.kalman_speed, cur.kalman_acceleration); report_time = System.currentTimeMillis(); - set_temperature(data.temperature()); - set_apogee_voltage(data.drogue_voltage()); - set_main_voltage(data.main_voltage()); - set_battery_voltage(data.battery_voltage()); - - set_pressure(data.pressure()); + set_temperature(cur.temperature()); + set_apogee_voltage(cur.drogue_voltage()); + set_main_voltage(cur.main_voltage()); + set_battery_voltage(cur.battery_voltage()); - set_tick(data.tick); - set_state(data.state); + set_pressure(cur.pressure()); - set_accel_g (data.accel_minus_g, data.accel_plus_g); - set_ground_accel(data.ground_accel); - set_accel (data.accel); + set_tick(cur.tick); + set_state(cur.state); - set_gps(data.gps, data.gps_sequence); + set_accel_g (cur.accel_minus_g, cur.accel_plus_g); + set_ground_accel(cur.ground_accel); + set_accel (cur.accel); - if (prev_state != null) { + if (cur.gps_sequence != gps_sequence) + set_gps(cur.gps, cur.gps_sequence); - if (data.kalman_speed != AltosRecord.MISSING) { - baro_speed = accel_speed = data.kalman_speed; - } else { - /* compute barometric speed */ - - double height_change = height - prev_state.height; - - double prev_baro_speed = prev_state.baro_speed; - if (prev_baro_speed == AltosRecord.MISSING) - prev_baro_speed = 0; - - if (time_change > 0) - baro_speed = (prev_baro_speed * 3 + (height_change / time_change)) / 4.0; - else - baro_speed = prev_state.baro_speed; - - double prev_accel_speed = prev_state.accel_speed; - - if (prev_accel_speed == AltosRecord.MISSING) - prev_accel_speed = 0; - - if (acceleration == AltosRecord.MISSING) { - /* Fill in mising acceleration value */ - accel_speed = baro_speed; - - if (time_change > 0 && accel_speed != AltosRecord.MISSING) - acceleration = (accel_speed - prev_accel_speed) / time_change; - else - acceleration = prev_state.acceleration; - } else { - /* compute accelerometer speed */ - accel_speed = prev_accel_speed + acceleration * time_change; - } - } - } else { - npad = 0; - ngps = 0; - gps = null; - gps_sequence = 0; - baro_speed = AltosRecord.MISSING; - accel_speed = AltosRecord.MISSING; - pad_alt = AltosRecord.MISSING; - max_baro_speed = 0; - max_accel_speed = 0; - max_height = 0; - max_acceleration = 0; - time_change = 0; - baro = new AltosMs5607(); - callsign = ""; - accel_plus_g = AltosRecord.MISSING; - accel_minus_g = AltosRecord.MISSING; - log_format = AltosRecord.MISSING; - serial = AltosRecord.MISSING; - } - - time = tick / 100.0; - - if (data.gps != null && data.gps_sequence != gps_sequence && (state < AltosLib.ao_flight_boost)) { - - /* Track consecutive 'good' gps reports, waiting for 10 of them */ - if (data.gps != null && data.gps.locked && data.gps.nsat >= 4) - set_npad(npad+1); - else - set_npad(0); - - /* Average GPS data while on the pad */ - if (data.gps != null && data.gps.locked && data.gps.nsat >= 4) { - if (ngps > 1 && state == AltosLib.ao_flight_pad) { - /* filter pad position */ - pad_lat = (pad_lat * 31.0 + data.gps.lat) / 32.0; - pad_lon = (pad_lon * 31.0 + data.gps.lon) / 32.0; - pad_alt = (pad_alt * 31.0 + data.gps.alt) / 32.0; - } else { - pad_lat = data.gps.lat; - pad_lon = data.gps.lon; - pad_alt = data.gps.alt; - } - ngps++; - } - } else { - if (ngps == 0 && ground_altitude != AltosRecord.MISSING) - pad_alt = ground_altitude; - } - - gps_sequence = data.gps_sequence; - - /* Only look at accelerometer data under boost */ - if (boost && acceleration != AltosRecord.MISSING && (max_acceleration == AltosRecord.MISSING || acceleration > max_acceleration)) - max_acceleration = acceleration; - if (boost && accel_speed != AltosRecord.MISSING && accel_speed > max_accel_speed) - max_accel_speed = accel_speed; - if (boost && baro_speed != AltosRecord.MISSING && baro_speed > max_baro_speed) - max_baro_speed = baro_speed; - - if (height != AltosRecord.MISSING && height > max_height) - max_height = height; - elevation = 0; - range = -1; - gps_height = 0; - if (data.gps != null) { - gps = data.gps; - if (ngps > 0 && gps.locked) { - double h = height; - - if (h == AltosRecord.MISSING) h = 0; - from_pad = new AltosGreatCircle(pad_lat, pad_lon, 0, gps.lat, gps.lon, h); - elevation = from_pad.elevation; - range = from_pad.range; - gps_height = gps.alt - pad_alt; - } - } } public AltosState clone() { - AltosState s = new AltosState(data, this); + AltosState s = new AltosState(); + s.copy(this); return s; } @@ -638,4 +755,8 @@ public class AltosState implements Cloneable { public AltosState (AltosRecord cur, AltosState prev) { init(cur, prev); } + + public AltosState () { + init(); + } }