* Track flight state from telemetry or eeprom data stream
*/
-package org.altusmetrum.altoslib_11;
-
-import java.io.*;
+package org.altusmetrum.altoslib_13;
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;
}
}
- 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)
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);
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() {
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() {
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) {
public double apogee_voltage;
public double main_voltage;
- public double ignitor_voltage[];
+ public double igniter_voltage[];
public AltosGPS gps;
- public AltosGPS temp_gps;
- public int temp_gps_sat_tick;
public boolean gps_pending;
- public AltosIMU imu;
- public AltosMag mag;
-
public static final int MIN_PAD_SAMPLES = 10;
public int npad;
public AltosGreatCircle from_pad;
public double elevation; /* from pad */
+ public double distance; /* distance along ground */
public double range; /* total distance */
public double gps_height;
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;
}
public void init() {
+ super.init();
+
set = 0;
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();
pyro_voltage = AltosLib.MISSING;
apogee_voltage = AltosLib.MISSING;
main_voltage = AltosLib.MISSING;
- ignitor_voltage = null;
+ igniter_voltage = null;
kalman_height = new AltosValue();
kalman_speed = new AltosValue();
kalman_acceleration = new AltosValue();
gps = null;
- temp_gps = null;
- temp_gps_sat_tick = 0;
gps_pending = false;
- 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;
+ 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;
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;
void update_gps() {
elevation = AltosLib.MISSING;
+ distance = AltosLib.MISSING;
range = AltosLib.MISSING;
if (gps == null)
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);
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 &&
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);
- }
-
- public void set_product(String product) {
- this.product = product;
+ 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;
- }
-
- 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;
+ super.set_state(state);
+ ascent = (AltosLib.ao_flight_boost <= state() &&
+ state() <= AltosLib.ao_flight_coast);
+ boost = (AltosLib.ao_flight_boost == state());
}
public int rssi() {
}
public void set_gps(AltosGPS gps) {
+ super.set_gps(gps);
if (gps != null) {
this.gps = gps;
update_gps();
}
}
-
- 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);
}
}
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);
}
}
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();
}
}
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) {
}
}
- public void set_ignitor_voltage(double[] voltage) {
- this.ignitor_voltage = voltage;
+ public void set_igniter_voltage(double[] voltage) {
+ this.igniter_voltage = voltage;
}
public void set_pyro_fired(int fired) {
this.pyro_fired = fired;
}
+ public AltosState() {
+ init();
+ }
+
public AltosState (AltosCalData cal_data) {
super(cal_data);
init();
projects / fw / altos / blobdiff