* Track flight state from telemetry or eeprom data stream
*/
-package org.altusmetrum.AltosLib;
+package org.altusmetrum.altoslib_1;
public class AltosState {
public AltosRecord data;
public double ground_altitude;
public double altitude;
public double height;
- public double speed;
public double acceleration;
public double battery;
public double temperature;
public double main_sense;
public double drogue_sense;
+ public double accel_speed;
public double baro_speed;
public double max_height;
public double max_acceleration;
- public double max_speed;
+ public double max_accel_speed;
public double max_baro_speed;
public AltosGPS gps;
public int speak_tick;
public double speak_altitude;
- public void init (AltosRecord cur, AltosState prev_state) {
- //int i;
- //AltosRecord prev;
+ public double speed() {
+ if (ascent)
+ return accel_speed;
+ else
+ return baro_speed;
+ }
+
+ public double max_speed() {
+ if (max_accel_speed != 0)
+ return max_accel_speed;
+ return max_baro_speed;
+ }
+ public void init (AltosRecord cur, AltosState prev_state) {
data = cur;
+ /* Discard previous state if it was for a different board */
+ if (prev_state != null && prev_state.data.serial != data.serial)
+ prev_state = null;
ground_altitude = data.ground_altitude();
- altitude = data.raw_altitude();
- height = data.filtered_height();
+
+ altitude = data.altitude();
+ if (altitude == AltosRecord.MISSING && data.gps != null)
+ altitude = data.gps.alt;
+
+ height = AltosRecord.MISSING;
+ if (data.kalman_height != AltosRecord.MISSING)
+ height = data.kalman_height;
+ else {
+ if (altitude != AltosRecord.MISSING && ground_altitude != AltosRecord.MISSING) {
+ double cur_height = altitude - ground_altitude;
+ if (prev_state == null || prev_state.height == AltosRecord.MISSING)
+ height = cur_height;
+ else
+ height = (prev_state.height * 15 + cur_height) / 16.0;
+ }
+ }
report_time = System.currentTimeMillis();
- acceleration = data.acceleration();
- speed = data.accel_speed();
+ if (data.kalman_acceleration != AltosRecord.MISSING)
+ acceleration = data.kalman_acceleration;
+ else
+ acceleration = data.acceleration();
temperature = data.temperature();
drogue_sense = data.drogue_voltage();
main_sense = data.main_voltage();
pad_alt = prev_state.pad_alt;
max_height = prev_state.max_height;
max_acceleration = prev_state.max_acceleration;
- max_speed = prev_state.max_speed;
+ max_accel_speed = prev_state.max_accel_speed;
max_baro_speed = prev_state.max_baro_speed;
imu = prev_state.imu;
mag = prev_state.mag;
time_change = (tick - prev_state.tick) / 100.0;
- /* compute barometric speed */
+ if (data.kalman_speed != AltosRecord.MISSING) {
+ baro_speed = accel_speed = data.kalman_speed;
+ } else {
+ /* compute barometric speed */
+
+ double height_change = height - prev_state.height;
+
+ double prev_baro_speed = prev_state.baro_speed;
+ if (prev_baro_speed == AltosRecord.MISSING)
+ prev_baro_speed = 0;
- double height_change = height - prev_state.height;
- if (data.speed != AltosRecord.MISSING)
- baro_speed = data.speed;
- else {
if (time_change > 0)
- baro_speed = (prev_state.baro_speed * 3 + (height_change / time_change)) / 4.0;
+ baro_speed = (prev_baro_speed * 3 + (height_change / time_change)) / 4.0;
else
baro_speed = prev_state.baro_speed;
+
+ double prev_accel_speed = prev_state.accel_speed;
+
+ if (prev_accel_speed == AltosRecord.MISSING)
+ prev_accel_speed = 0;
+
+ if (acceleration == AltosRecord.MISSING) {
+ /* Fill in mising acceleration value */
+ accel_speed = baro_speed;
+
+ if (time_change > 0 && accel_speed != AltosRecord.MISSING)
+ acceleration = (accel_speed - prev_accel_speed) / time_change;
+ else
+ acceleration = prev_state.acceleration;
+ } else {
+ /* compute accelerometer speed */
+ accel_speed = prev_accel_speed + acceleration * time_change;
+ }
}
} else {
npad = 0;
ngps = 0;
gps = null;
- baro_speed = 0;
+ baro_speed = AltosRecord.MISSING;
+ accel_speed = AltosRecord.MISSING;
+ pad_alt = AltosRecord.MISSING;
+ max_baro_speed = 0;
+ max_accel_speed = 0;
+ max_height = 0;
+ max_acceleration = 0;
time_change = 0;
}
time = tick / 100.0;
- if (cur.new_gps && (state == AltosLib.ao_flight_pad || state == AltosLib.ao_flight_idle)) {
+ if (cur.new_gps && (state < AltosLib.ao_flight_boost)) {
/* Track consecutive 'good' gps reports, waiting for 10 of them */
if (data.gps != null && data.gps.locked && data.gps.nsat >= 4)
/* Average GPS data while on the pad */
if (data.gps != null && data.gps.locked && data.gps.nsat >= 4) {
- if (ngps > 1) {
+ if (ngps > 1 && state == AltosLib.ao_flight_pad) {
/* filter pad position */
pad_lat = (pad_lat * 31.0 + data.gps.lat) / 32.0;
pad_lon = (pad_lon * 31.0 + data.gps.lon) / 32.0;
}
ngps++;
}
- } else
- pad_alt = ground_altitude;
+ } else {
+ if (ngps == 0 && ground_altitude != AltosRecord.MISSING)
+ pad_alt = ground_altitude;
+ }
data.new_gps = false;
boost = (AltosLib.ao_flight_boost == state);
/* Only look at accelerometer data under boost */
- if (boost && acceleration > max_acceleration && acceleration != AltosRecord.MISSING)
+ if (boost && acceleration != AltosRecord.MISSING && (max_acceleration == AltosRecord.MISSING || acceleration > max_acceleration))
max_acceleration = acceleration;
- if (boost && speed > max_speed && speed != AltosRecord.MISSING)
- max_speed = speed;
- if (boost && baro_speed > max_baro_speed && baro_speed != AltosRecord.MISSING)
+ if (boost && accel_speed != AltosRecord.MISSING && accel_speed > max_accel_speed)
+ max_accel_speed = accel_speed;
+ if (boost && baro_speed != AltosRecord.MISSING && baro_speed > max_baro_speed)
max_baro_speed = baro_speed;
- if (height > max_height && height != AltosRecord.MISSING)
+ if (height != AltosRecord.MISSING && height > max_height)
max_height = height;
- if (data.gps != null) {
- if (gps == null || !gps.locked || data.gps.locked)
- gps = data.gps;
- if (ngps > 0 && gps.locked) {
- from_pad = new AltosGreatCircle(pad_lat, pad_lon, gps.lat, gps.lon);
- }
- }
elevation = 0;
range = -1;
- if (ngps > 0) {
- gps_height = gps.alt - pad_alt;
- if (from_pad != null) {
- elevation = Math.atan2(height, from_pad.distance) * 180 / Math.PI;
- range = Math.sqrt(height * height + from_pad.distance * from_pad.distance);
+ gps_height = 0;
+ if (data.gps != null) {
+ gps = data.gps;
+ if (ngps > 0 && gps.locked) {
+ double h = height;
+
+ if (h == AltosRecord.MISSING) h = 0;
+ from_pad = new AltosGreatCircle(pad_lat, pad_lon, 0, gps.lat, gps.lon, h);
+ elevation = from_pad.elevation;
+ range = from_pad.range;
+ gps_height = gps.alt - pad_alt;
}
- } else {
- gps_height = 0;
}
}
projects / fw / altos / blobdiff