X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosState.java;h=aa3de432a75fa2e7feaaf9522d967ba1555213ed;hp=3b37a3d4b498fc90a8ba4e47cd1d6a03fd5681d8;hb=017ed54ff69ef2f7740ea2578e22bf72e88deafb;hpb=bd02349111ae0f39b320e6a10a330051ddc39fdf diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index 3b37a3d4..aa3de432 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -19,41 +19,63 @@ * Track flight state from telemetry or eeprom data stream */ -package org.altusmetrum.AltosLib; +package org.altusmetrum.altoslib_1; -public class AltosState { - public AltosRecord data; +public class AltosState implements Cloneable { + 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 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 speed; + public double pressure; public double acceleration; - public double battery; + public double battery_voltage; + public double pyro_voltage; public double temperature; - public double main_sense; - public double drogue_sense; - public double baro_speed; + public double apogee_voltage; + public double main_voltage; + public double speed; + + public double prev_height; + public double prev_speed; + public double prev_acceleration; public double max_height; public double max_acceleration; public double max_speed; - public double max_baro_speed; + + 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; public AltosMag mag; @@ -61,10 +83,11 @@ public class AltosState { public static final int MIN_PAD_SAMPLES = 10; public int npad; - public int ngps; public int gps_waiting; public boolean gps_ready; + public int ngps; + public AltosGreatCircle from_pad; public double elevation; /* from pad */ public double range; /* total distance */ @@ -76,133 +99,653 @@ public class AltosState { public int speak_tick; public double speak_altitude; - public void init (AltosRecord cur, AltosState prev_state) { - int i; - AltosRecord prev; + public String callsign; + public double accel_plus_g; + public double accel_minus_g; + public double accel; + public double ground_accel; + public double ground_accel_avg; + + public int log_format; - data = cur; + public AltosMs5607 baro; - ground_altitude = data.ground_altitude(); - altitude = data.raw_altitude(); - height = data.filtered_height(); + public AltosRecordCompanion companion; + + public double speed() { + return speed; + } + + public double max_speed() { + return max_speed; + } + + public void set_npad(int npad) { + this.npad = npad; + gps_waiting = MIN_PAD_SAMPLES - npad; + if (this.gps_waiting < 0) + gps_waiting = 0; + gps_ready = gps_waiting == 0; + } + + public void init() { + 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; + acceleration = AltosRecord.MISSING; + temperature = AltosRecord.MISSING; + + prev_height = AltosRecord.MISSING; + prev_speed = AltosRecord.MISSING; + prev_acceleration = AltosRecord.MISSING; + + battery_voltage = AltosRecord.MISSING; + pyro_voltage = AltosRecord.MISSING; + apogee_voltage = AltosRecord.MISSING; + main_voltage = AltosRecord.MISSING; + + speed = AltosRecord.MISSING; + + kalman_height = AltosRecord.MISSING; + kalman_speed = AltosRecord.MISSING; + kalman_acceleration = AltosRecord.MISSING; + + max_speed = 0; + max_height = 0; + max_acceleration = 0; + + gps = null; + temp_gps = null; + gps_sequence = 0; + gps_pending = false; + + imu = null; + mag = null; + + set_npad(0); + ngps = 0; + + from_pad = null; + elevation = AltosRecord.MISSING; + range = AltosRecord.MISSING; + gps_height = AltosRecord.MISSING; + + pad_lat = AltosRecord.MISSING; + pad_lon = AltosRecord.MISSING; + pad_alt = AltosRecord.MISSING; + + speak_tick = AltosRecord.MISSING; + speak_altitude = AltosRecord.MISSING; + + callsign = null; + + 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; + } - acceleration = data.acceleration(); - speed = data.accel_speed(); - temperature = data.temperature(); - drogue_sense = data.drogue_voltage(); - main_sense = data.main_voltage(); - battery = data.battery_voltage(); - tick = data.tick; - state = data.state; - - if (prev_state != null) { - - /* Preserve any existing gps data */ - npad = prev_state.npad; - ngps = prev_state.ngps; - gps = prev_state.gps; - pad_lat = prev_state.pad_lat; - pad_lon = prev_state.pad_lon; - pad_alt = prev_state.pad_alt; - max_height = prev_state.max_height; - max_acceleration = prev_state.max_acceleration; - max_speed = prev_state.max_speed; - max_baro_speed = prev_state.max_baro_speed; - imu = prev_state.imu; - mag = prev_state.mag; - - /* make sure the clock is monotonic */ - while (tick < prev_state.tick) - tick += 65536; - - time_change = (tick - prev_state.tick) / 100.0; - - /* compute barometric speed */ - - double height_change = height - prev_state.height; - if (data.speed != AltosRecord.MISSING) - baro_speed = data.speed; - else { - if (time_change > 0) - baro_speed = (prev_state.baro_speed * 3 + (height_change / time_change)) / 4.0; - else - baro_speed = prev_state.baro_speed; - } - } else { - npad = 0; - ngps = 0; - gps = null; - baro_speed = 0; - time_change = 0; + void copy(AltosState old) { + + record = null; + + if (old == null) { + init(); + return; } - time = tick / 100.0; + report_time = old.report_time; + time = old.time; + 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; + height = old.height; + pressure = old.pressure; + acceleration = old.acceleration; + battery_voltage = old.battery_voltage; + pyro_voltage = old.pyro_voltage; + temperature = old.temperature; + apogee_voltage = old.apogee_voltage; + main_voltage = old.main_voltage; + 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_speed = old.max_speed; + + kalman_height = old.kalman_height; + kalman_speed = old.kalman_speed; + kalman_acceleration = old.kalman_acceleration; + + if (old.gps != null) + 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(); + else + imu = null; + + if (old.mag != null) + mag = old.mag.clone(); + else + mag = null; + + npad = old.npad; + gps_waiting = old.gps_waiting; + gps_ready = old.gps_ready; + ngps = old.ngps; + + if (old.from_pad != null) + from_pad = old.from_pad.clone(); + else + from_pad = null; + + elevation = old.elevation; + range = old.range; + + gps_height = old.gps_height; + pad_lat = old.pad_lat; + pad_lon = old.pad_lon; + pad_alt = old.pad_alt; + + speak_tick = old.speak_tick; + speak_altitude = old.speak_altitude; + + callsign = old.callsign; + + 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; + companion = old.companion; + } - if (cur.new_gps && (state == AltosLib.ao_flight_pad || state == AltosLib.ao_flight_idle)) { + double altitude() { + if (altitude != AltosRecord.MISSING) + return altitude; + if (gps != null) + return gps.alt; + return AltosRecord.MISSING; + } - /* Track consecutive 'good' gps reports, waiting for 10 of them */ - if (data.gps != null && data.gps.locked && data.gps.nsat >= 4) - npad++; + void update_vertical_pos() { + + double alt = altitude(); + + if (state == AltosLib.ao_flight_pad && alt != AltosRecord.MISSING && ground_pressure == AltosRecord.MISSING) { + if (ground_altitude == AltosRecord.MISSING) + ground_altitude = alt; else - npad = 0; - - /* Average GPS data while on the pad */ - if (data.gps != null && data.gps.locked && data.gps.nsat >= 4) { - if (ngps > 1) { - /* 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; + ground_altitude = (ground_altitude * 7 + alt) / 8; + } + + if (kalman_height != AltosRecord.MISSING) + height = kalman_height; + else if (altitude != AltosRecord.MISSING && ground_altitude != AltosRecord.MISSING) + height = altitude - ground_altitude; + 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/2.0); + } + + void update_speed() { + if (kalman_speed != AltosRecord.MISSING) + speed = kalman_speed; + else if (state != AltosLib.ao_flight_invalid && + time_change != AltosRecord.MISSING) + { + if (ascent && acceleration != AltosRecord.MISSING) + { + if (prev_speed == AltosRecord.MISSING) + speed = acceleration * time_change; + else + speed = prev_speed + acceleration * time_change; + } + else if (height != AltosRecord.MISSING && + prev_height != AltosRecord.MISSING && + time_change != 0) + { + double new_speed = (height - prev_height) / time_change; + + if (prev_speed == AltosRecord.MISSING) + speed = new_speed; + else { + double filter = motion_filter_value(); + + speed = prev_speed * filter + new_speed * (1-filter); } - ngps++; } - } else - pad_alt = ground_altitude; - - gps_waiting = MIN_PAD_SAMPLES - npad; - if (gps_waiting < 0) - gps_waiting = 0; + } + if (acceleration == AltosRecord.MISSING) { + if (prev_speed != AltosRecord.MISSING && time_change != 0) { + double new_acceleration = (speed - prev_speed) / time_change; - gps_ready = gps_waiting == 0; + if (prev_acceleration == AltosRecord.MISSING) + acceleration = new_acceleration; + else { + double filter = motion_filter_value(); - ascent = (AltosLib.ao_flight_boost <= state && - state <= AltosLib.ao_flight_coast); - boost = (AltosLib.ao_flight_boost == state); + acceleration = prev_acceleration * filter + new_acceleration * (1-filter); + } + } + } + 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 == AltosRecord.MISSING) + return; + if (accel_plus_g == AltosRecord.MISSING) + return; + if (accel_minus_g == AltosRecord.MISSING) + return; + + double counts_per_g = (accel_minus_g - accel_plus_g) / 2.0; + double counts_per_mss = counts_per_g / 9.80665; + + acceleration = (ground - accel) / counts_per_mss; /* Only look at accelerometer data under boost */ - if (boost && acceleration > max_acceleration && acceleration != AltosRecord.MISSING) + if (boost && acceleration != AltosRecord.MISSING && (max_acceleration == AltosRecord.MISSING || acceleration > max_acceleration)) max_acceleration = acceleration; - if (boost && speed > max_speed && speed != AltosRecord.MISSING) - max_speed = speed; - if (boost && baro_speed > max_baro_speed && baro_speed != AltosRecord.MISSING) - max_baro_speed = baro_speed; + update_speed(); + } - if (height > max_height && height != AltosRecord.MISSING) - max_height = height; - if (data.gps != null) { - if (gps == null || !gps.locked || data.gps.locked) - gps = data.gps; - if (ngps > 0 && gps.locked) { - from_pad = new AltosGreatCircle(pad_lat, pad_lon, gps.lat, gps.lon); - } + 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; - if (ngps > 0) { + 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; - if (from_pad != null) { - elevation = Math.atan2(height, from_pad.distance) * 180 / Math.PI; - range = Math.sqrt(height * height + from_pad.distance * from_pad.distance); + } + } + + public void set_tick(int tick) { + if (tick != AltosRecord.MISSING) { + if (this.tick != AltosRecord.MISSING) { + while (tick < this.tick) + tick += 65536; + time_change = (tick - this.tick) / 100.0; + } else + time_change = 0; + this.tick = tick; + update_time(); + } + } + + 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; + ascent = (AltosLib.ao_flight_boost <= state && + state <= AltosLib.ao_flight_coast); + boost = (AltosLib.ao_flight_boost == state); + } + + } + + 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; + } + } + + public void set_ground_altitude(double ground_altitude) { + if (ground_altitude != AltosRecord.MISSING) { + this.ground_altitude = ground_altitude; + update_vertical_pos(); + } + } + + 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; + } + } + + public void set_kalman(double height, double speed, double acceleration) { + if (height != AltosRecord.MISSING) { + kalman_height = height; + kalman_speed = speed; + kalman_acceleration = acceleration; + update_vertical_pos(); + } + } + + public void set_pressure(double pressure) { + if (pressure != AltosRecord.MISSING) { + this.pressure = pressure; + set_altitude(AltosConvert.pressure_to_altitude(pressure)); + } + } + + 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; + this.accel_minus_g = accel_minus_g; + update_accel(); + } + } + public void set_ground_accel(double ground_accel) { + if (ground_accel != AltosRecord.MISSING) { + this.ground_accel = ground_accel; + update_accel(); + } + } + + 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; } - } else { - gps_height = 0; } + update_accel(); + } + + public void set_temperature(double temperature) { + if (temperature != AltosRecord.MISSING) { + this.temperature = temperature; + set |= set_data; + } + } + + public void set_battery_voltage(double battery_voltage) { + 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) { + this.pyro_voltage = pyro_voltage; + set |= set_data; + } + } + + public void set_apogee_voltage(double apogee_voltage) { + 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) { + 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(); + + record = cur; + + /* Discard previous state if it was for a different board */ + if (prev_state != null && prev_state.serial != cur.serial) + prev_state = null; + + copy(prev_state); + + set_ground_altitude(cur.ground_altitude()); + set_altitude(cur.altitude()); + + set_kalman(cur.kalman_height, cur.kalman_speed, cur.kalman_acceleration); + + report_time = System.currentTimeMillis(); + + set_temperature(cur.temperature()); + set_apogee_voltage(cur.drogue_voltage()); + set_main_voltage(cur.main_voltage()); + set_battery_voltage(cur.battery_voltage()); + + set_pressure(cur.pressure()); + + set_tick(cur.tick); + set_state(cur.state); + + set_accel_g (cur.accel_minus_g, cur.accel_plus_g); + set_ground_accel(cur.ground_accel); + set_accel (cur.accel); + + if (cur.gps_sequence != gps_sequence) + set_gps(cur.gps, cur.gps_sequence); + + } + + public AltosState clone() { + AltosState s = new AltosState(); + s.copy(this); + return s; } public AltosState(AltosRecord cur) { @@ -212,4 +755,8 @@ public class AltosState { public AltosState (AltosRecord cur, AltosState prev) { init(cur, prev); } + + public AltosState () { + init(); + } }