X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosState.java;h=9ee3d57ddf814fe340ef53df61fa2e31758d5cae;hp=d3929b8f7e047480ddcd02cef4eed0953863f26c;hb=dd72c9144b207b12150eb6a7ffb012f217f37374;hpb=0f32d1a2e2fb5989d124189a7eb1156a00b0ad8f diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index d3929b8f..9ee3d57d 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -20,9 +20,7 @@ * Track flight state from telemetry or eeprom data stream */ -package org.altusmetrum.altoslib_11; - -import java.io.*; +package org.altusmetrum.altoslib_12; public class AltosState extends AltosDataListener { @@ -43,8 +41,6 @@ public class AltosState extends AltosDataListener { public int rssi; public int status; - public double time; - class AltosValue { double value; double prev_value; @@ -288,7 +284,6 @@ public class AltosState extends AltosDataListener { } } - private int state; public boolean landed; public boolean ascent; /* going up? */ public boolean boost; /* under power */ @@ -506,23 +501,7 @@ public class AltosState extends AltosDataListener { pressure.set(p, time); } - 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() { @@ -698,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; @@ -711,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; @@ -720,15 +697,8 @@ public class AltosState extends AltosDataListener { public int speak_tick; public double speak_altitude; - public String callsign; - public String firmware_version; - public double ground_accel; - public int log_format; - public int log_space; - public String product; - public AltosCompanion companion; public int pyro_fired; @@ -756,7 +726,6 @@ public class AltosState extends AltosDataListener { ground_pressure = new AltosGroundPressure(); altitude = new AltosAltitude(); pressure = new AltosPressure(); - thrust = new AltosForce(); speed = new AltosSpeed(); acceleration = new AltosAccel(); orient = new AltosCValue(); @@ -775,24 +744,31 @@ public class AltosState extends AltosDataListener { gps = null; gps_pending = false; - imu = null; last_imu_time = AltosLib.MISSING; rotation = null; - ground_rotation = null; - - mag = null; accel_ground_along = AltosLib.MISSING; accel_ground_across = AltosLib.MISSING; accel_ground_through = AltosLib.MISSING; - pad_orientation = 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; @@ -810,15 +786,8 @@ public class AltosState extends AltosDataListener { speak_tick = AltosLib.MISSING; speak_altitude = AltosLib.MISSING; - callsign = null; - firmware_version = null; - ground_accel = AltosLib.MISSING; - log_format = AltosLib.MISSING; - log_space = AltosLib.MISSING; - product = null; - companion = null; pyro_fired = 0; @@ -842,6 +811,7 @@ public class AltosState extends AltosDataListener { void update_gps() { elevation = AltosLib.MISSING; + distance = AltosLib.MISSING; range = AltosLib.MISSING; if (gps == null) @@ -883,6 +853,7 @@ 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; } } @@ -933,21 +904,16 @@ public class AltosState extends AltosDataListener { } } - public AltosRotation rotation; - public AltosRotation ground_rotation; - - 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 - 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), - pad_orientation); - ground_rotation = 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); orient.set_computed(rotation.tilt(), time); } } @@ -959,29 +925,18 @@ public class AltosState extends AltosDataListener { update_pad_rotation(); } - public void set_pad_orientation(int pad_orientation) { - this.pad_orientation = pad_orientation; - update_pad_rotation(); - } - 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 && 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); } } @@ -992,8 +947,8 @@ public class AltosState extends AltosDataListener { public void set_gyro(double roll, double pitch, double yaw) { gyro_roll = roll; - gyro_pitch = roll; - gyro_roll = roll; + gyro_pitch = pitch; + gyro_yaw = yaw; update_orient(); } @@ -1043,15 +998,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) { @@ -1108,6 +1059,11 @@ public class AltosState extends AltosDataListener { this.pyro_fired = fired; } + public AltosState() { + Thread.dumpStack(); + init(); + } + public AltosState (AltosCalData cal_data) { super(cal_data); init();