X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosState.java;h=036c1652ed31c0c157f9c5e6b80cf3450fc1e44f;hp=0716b892a1c1b97d97b8e9337195f3b79721b086;hb=03ec3e4c2247b6520c728b34805ad500be547a25;hpb=aa0300f11fe5b7f1d3e548e29dc809b803aa1f7a diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index 0716b892..036c1652 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -22,8 +22,6 @@ package org.altusmetrum.altoslib_11; -import java.io.*; - public class AltosState extends AltosDataListener { public static final int set_position = 1; @@ -43,10 +41,6 @@ public class AltosState extends AltosDataListener { public int rssi; public int status; - public double time; - public double prev_time; - public double time_change; - class AltosValue { double value; double prev_value; @@ -290,17 +284,9 @@ public class AltosState extends AltosDataListener { } } - private int state; - public int altitude_32; - public int receiver_serial; public boolean landed; public boolean ascent; /* going up? */ public boolean boost; /* under power */ - public int config_major; - public int config_minor; - public int apogee_delay; - public int main_deploy; - public int flight_log_max; private double pressure_to_altitude(double p) { if (p == AltosLib.MISSING) @@ -515,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() { @@ -729,17 +699,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 AltosMs5607 baro; - public AltosCompanion companion; public int pyro_fired; @@ -755,27 +716,18 @@ public class AltosState extends AltosDataListener { public void init() { set = 0; - System.out.printf("state init\n"); received_time = System.currentTimeMillis(); time = AltosLib.MISSING; - time_change = AltosLib.MISSING; - prev_time = AltosLib.MISSING; state = AltosLib.ao_flight_invalid; landed = false; boost = false; rssi = AltosLib.MISSING; status = 0; - config_major = AltosLib.MISSING; - config_minor = AltosLib.MISSING; - apogee_delay = AltosLib.MISSING; - main_deploy = AltosLib.MISSING; - flight_log_max = AltosLib.MISSING; ground_altitude = new AltosCValue(); ground_pressure = new AltosGroundPressure(); altitude = new AltosAltitude(); pressure = new AltosPressure(); - thrust = new AltosForce(); speed = new AltosSpeed(); acceleration = new AltosAccel(); orient = new AltosCValue(); @@ -797,19 +749,13 @@ public class AltosState extends AltosDataListener { 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; - set_npad(0); ngps = 0; @@ -832,18 +778,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; - receiver_serial = AltosLib.MISSING; - altitude_32 = AltosLib.MISSING; - - baro = null; companion = null; pyro_fired = 0; @@ -916,10 +852,6 @@ public class AltosState extends AltosDataListener { 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; @@ -933,85 +865,8 @@ public class AltosState extends AltosDataListener { return state; } - public void set_log_format(int log_format) { - this.log_format = log_format; - switch (log_format) { - case AltosLib.AO_LOG_FORMAT_TELEGPS: - this.state = AltosLib.ao_flight_stateless; - break; - } - } - - 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; - } - - public void set_config(int major, int minor, int flight_log_max) { - config_major = major; - config_minor = minor; - this.flight_log_max = flight_log_max; - } - - public void set_callsign(String callsign) { - this.callsign = callsign; - } - - public void set_firmware_version(String version) { - 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 rs = receiver_serial; init(); - receiver_serial = rs; - } - -// public void set_flight(int flight) { -// -// /* When the flight changes, reset the state */ -// if (flight != AltosLib.MISSING) { -// if (this.flight != AltosLib.MISSING && -// this.flight != flight) { -// re_init(); -// } -// this.flight = flight; -// } -// } -// -// public void set_serial(int serial) { -// /* When the serial changes, reset the state */ -// if (serial != AltosLib.MISSING) { -// if (this.serial != AltosLib.MISSING && -// this.serial != serial) { -// re_init(); -// } -// this.serial = serial; -// } -// } -// -// public void set_receiver_serial(int serial) { -// if (serial != AltosLib.MISSING) -// 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() { @@ -1040,32 +895,16 @@ public class AltosState extends AltosDataListener { } - 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; + 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); } } @@ -1077,28 +916,17 @@ 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()); + 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 & pitch != AltosLib.MISSING && rotation != null) { + if (t > 0 && pitch != AltosLib.MISSING && rotation != null) { rotation.rotate(t, pitch, yaw, roll); orient.set_computed(rotation.tilt(), time); } @@ -1110,8 +938,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_roll = yaw; update_orient(); } @@ -1226,8 +1054,15 @@ public class AltosState extends AltosDataListener { this.pyro_fired = fired; } + public AltosState() { + Thread.dumpStack(); + init(); + } + public AltosState (AltosCalData cal_data) { super(cal_data); + if (cal_data == null) + Thread.dumpStack(); init(); } }