X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosState.java;h=7a64f8a188182704c5aa6fbd01e17fb627eef5e5;hp=036c1652ed31c0c157f9c5e6b80cf3450fc1e44f;hb=e64b1bc108bd75bcd6271631e48abde84af4631f;hpb=03ec3e4c2247b6520c728b34805ad500be547a25 diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index 036c1652..7a64f8a1 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -20,7 +20,7 @@ * Track flight state from telemetry or eeprom data stream */ -package org.altusmetrum.altoslib_11; +package org.altusmetrum.altoslib_13; public class AltosState extends AltosDataListener { @@ -480,7 +480,7 @@ public class AltosState extends AltosDataListener { class AltosPressure extends AltosValue { void set(double p, double time) { super.set(p, time); - if (state == AltosLib.ao_flight_pad) + if (state() == AltosLib.ao_flight_pad) ground_pressure.set_filtered(p, time); double a = pressure_to_altitude(p); altitude.set_computed(a, time); @@ -557,7 +557,7 @@ public class AltosState extends AltosDataListener { class AltosSpeed extends AltosCValue { boolean can_max() { - return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless; + return state() < AltosLib.ao_flight_fast || state() == AltosLib.ao_flight_stateless; } void set_accel() { @@ -615,7 +615,7 @@ public class AltosState extends AltosDataListener { class AltosAccel extends AltosCValue { boolean can_max() { - return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless; + return state() < AltosLib.ao_flight_fast || state() == AltosLib.ao_flight_stateless; } void set_measured(double a, double time) { @@ -677,9 +677,6 @@ public class AltosState extends AltosDataListener { public AltosGPS gps; public boolean gps_pending; - public AltosIMU imu; - public AltosMag mag; - public static final int MIN_PAD_SAMPLES = 10; public int npad; @@ -690,6 +687,7 @@ public class AltosState extends AltosDataListener { public AltosGreatCircle from_pad; public double elevation; /* from pad */ + public double distance; /* distance along ground */ public double range; /* total distance */ public double gps_height; @@ -714,11 +712,11 @@ public class AltosState extends AltosDataListener { } public void init() { + super.init(); + set = 0; received_time = System.currentTimeMillis(); - time = AltosLib.MISSING; - state = AltosLib.ao_flight_invalid; landed = false; boost = false; rssi = AltosLib.MISSING; @@ -746,21 +744,31 @@ public class AltosState extends AltosDataListener { gps = null; gps_pending = false; - imu = null; last_imu_time = AltosLib.MISSING; rotation = null; - mag = null; - accel_ground_along = AltosLib.MISSING; accel_ground_across = AltosLib.MISSING; accel_ground_through = AltosLib.MISSING; + accel_along = AltosLib.MISSING; + accel_across = AltosLib.MISSING; + accel_through = AltosLib.MISSING; + + gyro_roll = AltosLib.MISSING; + gyro_pitch = AltosLib.MISSING; + gyro_yaw = AltosLib.MISSING; + + mag_along = AltosLib.MISSING; + mag_across = AltosLib.MISSING; + mag_through = AltosLib.MISSING; + set_npad(0); ngps = 0; from_pad = null; elevation = AltosLib.MISSING; + distance = AltosLib.MISSING; range = AltosLib.MISSING; gps_height = AltosLib.MISSING; @@ -803,6 +811,7 @@ public class AltosState extends AltosDataListener { void update_gps() { elevation = AltosLib.MISSING; + distance = AltosLib.MISSING; range = AltosLib.MISSING; if (gps == null) @@ -810,9 +819,9 @@ public class AltosState extends AltosDataListener { if (gps.locked && gps.nsat >= 4) { /* Track consecutive 'good' gps reports, waiting for 10 of them */ - if (state == AltosLib.ao_flight_pad || state == AltosLib.ao_flight_stateless) { + if (state() == AltosLib.ao_flight_pad || state() == AltosLib.ao_flight_stateless) { set_npad(npad+1); - if (pad_lat != AltosLib.MISSING && (npad < 10 || state == AltosLib.ao_flight_pad)) { + 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); @@ -833,6 +842,8 @@ public class AltosState extends AltosDataListener { gps.climb_rate * gps.climb_rate), time); if (gps.course != AltosLib.MISSING) gps_course.set(gps.course, time); + } else if (state() == AltosLib.ao_flight_pad || state() == AltosLib.ao_flight_stateless) { + set_npad(0); } if (gps.lat != 0 && gps.lon != 0 && pad_lat != AltosLib.MISSING && @@ -844,29 +855,20 @@ public class AltosState extends AltosDataListener { h = 0; from_pad = new AltosGreatCircle(pad_lat, pad_lon, 0, gps.lat, gps.lon, h); elevation = from_pad.elevation; + distance = from_pad.distance; range = from_pad.range; } } public String state_name() { - return AltosLib.state_name(state); + 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 int state() { - return state; - } - - private void re_init() { - init(); + super.set_state(state); + ascent = (AltosLib.ao_flight_boost <= state() && + state() <= AltosLib.ao_flight_coast); + boost = (AltosLib.ao_flight_boost == state()); } public int rssi() { @@ -887,6 +889,7 @@ public class AltosState extends AltosDataListener { } public void set_gps(AltosGPS gps) { + super.set_gps(gps); if (gps != null) { this.gps = gps; update_gps(); @@ -894,17 +897,16 @@ public class AltosState extends AltosDataListener { } } - public AltosRotation rotation; public double accel_ground_along, accel_ground_across, accel_ground_through; void update_pad_rotation() { - if (cal_data.pad_orientation != AltosLib.MISSING && accel_ground_along != AltosLib.MISSING) { - rotation = new AltosRotation(AltosIMU.convert_accel(accel_ground_across - cal_data.accel_zero_across), - AltosIMU.convert_accel(accel_ground_through - cal_data.accel_zero_through), - AltosIMU.convert_accel(accel_ground_along - cal_data.accel_zero_along), - cal_data.pad_orientation); + if (cal_data().pad_orientation != AltosLib.MISSING && accel_ground_along != AltosLib.MISSING) { + rotation = new AltosRotation(AltosIMU.convert_accel(accel_ground_across - cal_data().accel_zero_across), + AltosIMU.convert_accel(accel_ground_through - cal_data().accel_zero_through), + AltosIMU.convert_accel(accel_ground_along - cal_data().accel_zero_along), + cal_data().pad_orientation); orient.set_computed(rotation.tilt(), time); } } @@ -922,12 +924,12 @@ public class AltosState extends AltosDataListener { if (last_imu_time != AltosLib.MISSING) { double t = time - last_imu_time; - double pitch = AltosConvert.degrees_to_radians(gyro_pitch()); - double yaw = AltosConvert.degrees_to_radians(gyro_yaw()); - double roll = AltosConvert.degrees_to_radians(gyro_roll()); + if (t > 0 && gyro_pitch != AltosLib.MISSING && rotation != null) { + double pitch = AltosConvert.degrees_to_radians(gyro_pitch) * t; + double yaw = AltosConvert.degrees_to_radians(gyro_yaw) * t; + double roll = AltosConvert.degrees_to_radians(gyro_roll) * t; - if (t > 0 && pitch != AltosLib.MISSING && rotation != null) { - rotation.rotate(t, pitch, yaw, roll); + rotation.rotate(pitch, yaw, roll); orient.set_computed(rotation.tilt(), time); } } @@ -939,7 +941,7 @@ public class AltosState extends AltosDataListener { public void set_gyro(double roll, double pitch, double yaw) { gyro_roll = roll; gyro_pitch = pitch; - gyro_roll = yaw; + gyro_yaw = yaw; update_orient(); } @@ -989,15 +991,11 @@ public class AltosState extends AltosDataListener { } public double mag_across() { - if (mag != null) - return AltosMag.convert_gauss(mag.across); - return AltosLib.MISSING; + return mag_across; } public double mag_through() { - if (mag != null) - return AltosMag.convert_gauss(mag.through); - return AltosLib.MISSING; + return mag_through; } public void set_companion(AltosCompanion companion) { @@ -1055,14 +1053,11 @@ public class AltosState extends AltosDataListener { } public AltosState() { - Thread.dumpStack(); init(); } public AltosState (AltosCalData cal_data) { super(cal_data); - if (cal_data == null) - Thread.dumpStack(); init(); } }