* Track flight state from telemetry or eeprom data stream
*/
-package org.altusmetrum.altoslib_12;
+package org.altusmetrum.altoslib_14;
public class AltosState extends AltosDataListener {
class AltosGpsGroundAltitude extends AltosValue {
void set(double a, double t) {
super.set(a, t);
- pad_alt = value();
+
gps_altitude.set_gps_height();
}
void set_filtered(double a, double t) {
super.set_filtered(a, t);
- pad_alt = value();
gps_altitude.set_gps_height();
}
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);
}
public double height() {
- double k = kalman_height.value();
- if (k != AltosLib.MISSING)
- return k;
-
double b = baro_height();
if (b != AltosLib.MISSING)
return b;
+ double k = kalman_height.value();
+ if (k != AltosLib.MISSING)
+ return k;
+
return gps_height();
}
public double max_height() {
- double k = kalman_height.max();
- if (k != AltosLib.MISSING)
- return k;
-
double a = altitude.max();
double g = ground_altitude();
if (a != AltosLib.MISSING && g != AltosLib.MISSING)
return a - g;
+
+ double k = kalman_height.max();
+ if (k != AltosLib.MISSING)
+ return k;
+
return max_gps_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 gps_height;
- public double pad_lat, pad_lon, pad_alt;
+ public double pad_lat, pad_lon;
public int speak_tick;
public double speak_altitude;
}
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;
pad_lat = AltosLib.MISSING;
pad_lon = AltosLib.MISSING;
- pad_alt = AltosLib.MISSING;
gps_altitude = new AltosGpsAltitude();
gps_ground_altitude = new AltosGpsGroundAltitude();
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 &&
}
}
- 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 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() {
received_time = ms;
}
- public void set_gps(AltosGPS gps) {
+ public void set_gps(AltosGPS gps, boolean set_location, boolean set_sats) {
+ super.set_gps(gps, set_location, set_sats);
if (gps != null) {
this.gps = gps;
update_gps();
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),
+ rotation = new AltosRotation(accel_ground_across,
+ accel_ground_through,
+ accel_ground_along,
cal_data().pad_orientation);
orient.set_computed(rotation.tilt(), time);
}
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);
}
}
}
public AltosState() {
- Thread.dumpStack();
init();
}