else if (has_sensor_mm(config_data))
record = sensor_mm(config_data);
else
- record = new AltosRecord();
+ record = new AltosRecordNone();
if (has_gps(config_data))
gps = new AltosGPSQuery(link, config_data);
package org.altusmetrum.AltosLib;
-public class AltosRecord implements Comparable <AltosRecord>, Cloneable {
+public abstract class AltosRecord implements Comparable <AltosRecord>, Cloneable {
public static final int seen_flight = 1;
public static final int seen_sensor = 2;
public int state;
public int tick;
- /* Current flight dynamic state */
- public double acceleration; /* m/s² */
- public double speed; /* m/s */
- public double height; /* m */
-
public AltosGPS gps;
public boolean new_gps;
public AltosRecordCompanion companion;
+ /* Telemetry sources have these values recorded from the flight computer */
+ public double kalman_height;
+ public double kalman_speed;
+ public double kalman_acceleration;
+
/*
* Abstract methods that convert record data
* to standard units:
* temperature: °C
*/
- public double raw_pressure() { return MISSING; }
-
- public double filtered_pressure() { return MISSING; }
-
- public double ground_pressure() { return MISSING; }
-
- public double battery_voltage() { return MISSING; }
+ abstract public double pressure();
+ abstract public double ground_pressure();
+ abstract public double acceleration();
- public double main_voltage() { return MISSING; }
+ public double altitude() {
+ double p = pressure();
- public double drogue_voltage() { return MISSING; }
-
- public double temperature() { return MISSING; }
-
- public double acceleration() { return MISSING; }
-
- public double accel_speed() { return MISSING; }
-
- public AltosIMU imu() { return null; }
-
- public AltosMag mag() { return null; }
-
- /*
- * Convert various pressure values to altitude
- */
-
- public double raw_altitude() {
- double p = raw_pressure();
if (p == MISSING)
return MISSING;
return AltosConvert.pressure_to_altitude(p);
}
public double ground_altitude() {
- double p = ground_pressure();
+ double p = ground_pressure();
+
if (p == MISSING)
return MISSING;
return AltosConvert.pressure_to_altitude(p);
}
- public double filtered_altitude() {
- double ga = ground_altitude();
- if (height != MISSING && ga != MISSING)
- return height + ga;
+ public double height() {
+ double g = ground_altitude();
+ double a = altitude();
- double p = filtered_pressure();
- if (p == MISSING)
- return raw_altitude();
- return AltosConvert.pressure_to_altitude(p);
+ if (g == MISSING)
+ return MISSING;
+ if (a == MISSING)
+ return MISSING;
+ return a - g;
}
- public double filtered_height() {
- if (height != MISSING)
- return height;
+ public double battery_voltage() { return MISSING; }
- double f = filtered_altitude();
- double g = ground_altitude();
- if (f == MISSING || g == MISSING)
- return MISSING;
- return f - g;
- }
+ public double main_voltage() { return MISSING; }
- public double raw_height() {
- double r = raw_altitude();
- double g = ground_altitude();
+ public double drogue_voltage() { return MISSING; }
- if (r == MISSING || g == MISSING)
- return height;
- return r - g;
- }
+ public double temperature() { return MISSING; }
+
+ public AltosIMU imu() { return null; }
+
+ public AltosMag mag() { return null; }
public String state() {
return AltosLib.state_name(state);
status = old.status;
state = old.state;
tick = old.tick;
- acceleration = old.acceleration;
- speed = old.speed;
- height = old.height;
gps = new AltosGPS(old.gps);
new_gps = old.new_gps;
companion = old.companion;
+ kalman_acceleration = old.kalman_acceleration;
+ kalman_speed = old.kalman_speed;
+ kalman_height = old.kalman_height;
}
public AltosRecord clone() {
status = 0;
state = AltosLib.ao_flight_startup;
tick = 0;
- acceleration = MISSING;
- speed = MISSING;
- height = MISSING;
gps = new AltosGPS();
new_gps = false;
companion = null;
+
+ kalman_acceleration = MISSING;
+ kalman_speed = MISSING;
+ kalman_height = MISSING;
}
}
public class AltosRecordMM extends AltosRecord {
+ /* Sensor values */
public int accel;
public int pres;
public int temp;
return raw / 4095.0;
}
- public double raw_pressure() {
+ public double pressure() {
if (pres != MISSING)
return pres;
return MISSING;
}
- public double filtered_pressure() {
- return raw_pressure();
- }
-
public double ground_pressure() {
if (ground_pres != MISSING)
return ground_pres;
}
public double acceleration() {
- if (acceleration != MISSING)
- return acceleration;
-
if (ground_accel == MISSING || accel == MISSING)
return MISSING;
return (ground_accel - accel) / accel_counts_per_mss();
}
- public double accel_speed() {
- if (speed != MISSING)
- return speed;
- if (flight_vel == MISSING)
- return MISSING;
- return flight_vel / (accel_counts_per_mss() * 100.0);
- }
-
public void copy (AltosRecordMM old) {
super.copy(old);
package org.altusmetrum.AltosLib;
public class AltosRecordTM extends AltosRecord {
+
+ /* Sensor values */
public int accel;
public int pres;
public int temp;
return ((count / 16.0) / 2047.0 + 0.095) / 0.009 * 1000.0;
}
- public double raw_pressure() {
+ public double pressure() {
if (pres == MISSING)
return MISSING;
return barometer_to_pressure(pres);
}
- public double filtered_pressure() {
- if (flight_pres == MISSING)
- return MISSING;
- return barometer_to_pressure(flight_pres);
- }
-
public double ground_pressure() {
if (ground_pres == MISSING)
return MISSING;
}
public double acceleration() {
- if (acceleration != MISSING)
- return acceleration;
-
if (ground_accel == MISSING || accel == MISSING)
return MISSING;
return (ground_accel - accel) / accel_counts_per_mss();
}
- public double accel_speed() {
- if (speed != MISSING)
- return speed;
- if (flight_vel == MISSING)
- return MISSING;
- return flight_vel / (accel_counts_per_mss() * 100.0);
- }
-
public void copy(AltosRecordTM old) {
super.copy(old);
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;
ground_altitude = data.ground_altitude();
- altitude = data.raw_altitude();
- height = data.filtered_height();
+
+ altitude = data.altitude();
+
+ if (data.kalman_height != AltosRecord.MISSING)
+ height = data.kalman_height;
+ else {
+ if (prev_state != null)
+ height = (prev_state.height * 15 + altitude - ground_altitude) / 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;
- if (data.speed != AltosRecord.MISSING)
- baro_speed = data.speed;
- else {
+ double height_change = height - prev_state.height;
if (time_change > 0)
baro_speed = (prev_state.baro_speed * 3 + (height_change / time_change)) / 4.0;
else
baro_speed = prev_state.baro_speed;
+
+ if (acceleration == AltosRecord.MISSING) {
+ /* Fill in mising acceleration value */
+ accel_speed = baro_speed;
+ if (time_change > 0)
+ acceleration = (accel_speed - prev_state.accel_speed) / time_change;
+ else
+ acceleration = prev_state.acceleration;
+ } else {
+ /* compute accelerometer speed */
+ accel_speed = prev_state.accel_speed + acceleration * time_change;
+ }
}
+
} else {
npad = 0;
ngps = 0;
gps = null;
baro_speed = 0;
+ accel_speed = 0;
time_change = 0;
+ if (acceleration == AltosRecord.MISSING)
+ acceleration = 0;
}
time = tick / 100.0;
/* Only look at accelerometer data under boost */
if (boost && acceleration > max_acceleration && acceleration != AltosRecord.MISSING)
max_acceleration = acceleration;
- if (boost && speed > max_speed && speed != AltosRecord.MISSING)
- max_speed = speed;
+ if (boost && accel_speed > max_accel_speed && accel_speed != AltosRecord.MISSING)
+ max_accel_speed = accel_speed;
if (boost && baro_speed > max_baro_speed && baro_speed != AltosRecord.MISSING)
max_baro_speed = baro_speed;
record.accel_minus_g = map.get_int(AO_TELEM_CAL_ACCEL_MINUS, AltosRecord.MISSING);
/* flight computer values */
- record.acceleration = map.get_double(AO_TELEM_KALMAN_ACCEL, AltosRecord.MISSING, 1/16.0);
- record.speed = map.get_double(AO_TELEM_KALMAN_SPEED, AltosRecord.MISSING, 1/16.0);
- record.height = map.get_int(AO_TELEM_KALMAN_HEIGHT, AltosRecord.MISSING);
+ record.kalman_acceleration = map.get_double(AO_TELEM_KALMAN_ACCEL, AltosRecord.MISSING, 1/16.0);
+ record.kalman_speed = map.get_double(AO_TELEM_KALMAN_SPEED, AltosRecord.MISSING, 1/16.0);
+ record.kalman_height = map.get_int(AO_TELEM_KALMAN_HEIGHT, AltosRecord.MISSING);
record.flight_accel = map.get_int(AO_TELEM_ADHOC_ACCEL, AltosRecord.MISSING);
record.flight_vel = map.get_int(AO_TELEM_ADHOC_SPEED, AltosRecord.MISSING);
/* Old TeleDongle code with kalman-reporting TeleMetrum code */
if ((record.flight_vel & 0xffff0000) == 0x80000000) {
- record.speed = ((short) record.flight_vel) / 16.0;
- record.acceleration = record.flight_accel / 16.0;
- record.height = record.flight_pres;
+ record.kalman_speed = ((short) record.flight_vel) / 16.0;
+ record.kalman_acceleration = record.flight_accel / 16.0;
+ record.kalman_height = record.flight_pres;
record.flight_vel = AltosRecord.MISSING;
record.flight_pres = AltosRecord.MISSING;
record.flight_accel = AltosRecord.MISSING;
record.accel_minus_g = int16(19);
if (uint16(11) == 0x8000) {
- record.acceleration = int16(5);
- record.speed = int16(9);
- record.height = int16(13);
+ record.kalman_acceleration = int16(5);
+ record.kalman_speed = int16(9);
+ record.kalman_height = int16(13);
record.flight_accel = AltosRecord.MISSING;
record.flight_vel = AltosRecord.MISSING;
record.flight_pres = AltosRecord.MISSING;
record.flight_accel = int16(5);
record.flight_vel = uint32(9);
record.flight_pres = int16(13);
- record.acceleration = AltosRecord.MISSING;
- record.speed = AltosRecord.MISSING;
- record.height = AltosRecord.MISSING;
+ record.kalman_acceleration = AltosRecord.MISSING;
+ record.kalman_speed = AltosRecord.MISSING;
+ record.kalman_height = AltosRecord.MISSING;
}
record.gps = null;
next.accel_plus_g = accel_plus_g;
next.accel_minus_g = accel_minus_g;
- next.acceleration = acceleration / 16.0;
- next.speed = speed / 16.0;
- next.height = height;
+ next.kalman_acceleration = acceleration / 16.0;
+ next.kalman_speed = speed / 16.0;
+ next.kalman_height = height;
next.seen |= AltosRecord.seen_flight | AltosRecord.seen_temp_volt;
if (previous != null)
next = previous.clone();
else
- next = new AltosRecord();
+ next = new AltosRecordNone();
next.serial = serial;
next.tick = tick;
return next;
next.main = AltosRecord.MISSING;
}
- next.acceleration = acceleration / 16.0;
- next.speed = speed / 16.0;
- next.height = height;
+ next.kalman_acceleration = acceleration / 16.0;
+ next.kalman_speed = speed / 16.0;
+ next.kalman_height = height;
next.ground_pres = ground_pres;
if (type == packet_type_TM_sensor) {
$(SRC)/AltosRecordCompanion.java \
$(SRC)/AltosRecordIterable.java \
$(SRC)/AltosRecord.java \
+ $(SRC)/AltosRecordNone.java \
$(SRC)/AltosRecordTM.java \
$(SRC)/AltosRecordMM.java \
$(SRC)/AltosReplayReader.java \
class Speed extends AscentValueHold {
void show (AltosState state, int crc_errors) {
- double speed = state.speed;
+ double speed = state.accel_speed;
if (!state.ascent)
speed = state.baro_speed;
show(AltosConvert.speed, speed);
void write_basic(AltosRecord record) {
out.printf("%8.2f,%10.2f,%8.2f,%8.2f,%8.2f,%8.2f,%5.1f,%5.2f,%5.2f,%5.2f",
record.acceleration(),
- record.raw_pressure(),
- record.raw_altitude(),
- record.raw_height(),
- record.accel_speed(),
+ record.pressure(),
+ record.altitude(),
+ record.height(),
+ state.accel_speed,
state.baro_speed,
record.temperature(),
record.battery_voltage(),
String state_name();
double acceleration();
- double pressure();
- double altitude();
double height();
- double accel_speed();
- double baro_speed();
+ double speed();
double temperature();
double battery_voltage();
double drogue_voltage();
class AltosDataPointReader implements Iterable<AltosDataPoint> {
Iterator<AltosRecord> iter;
AltosState state;
- AltosRecord record;
boolean has_gps;
boolean has_accel;
boolean has_ignite;
public AltosDataPointReader(AltosRecordIterable reader) {
this.iter = reader.iterator();
this.state = null;
- has_accel = reader.has_accel();
+ has_accel = true;
has_gps = reader.has_gps();
has_ignite = reader.has_ignite();
}
private void read_next_record()
throws NoSuchElementException
{
- record = iter.next();
- state = new AltosState(record, state);
+ state = new AltosState(iter.next(), state);
}
private AltosDataPoint current_dp() {
- assert this.record != null;
+ assert this.state != null;
return new AltosDataPoint() {
- public int version() { return record.version; }
- public int serial() { return record.serial; }
- public int flight() { return record.flight; }
- public String callsign() { return record.callsign; }
- public double time() { return record.time; }
- public double rssi() { return record.rssi; }
+ public int version() { return state.data.version; }
+ public int serial() { return state.data.serial; }
+ public int flight() { return state.data.flight; }
+ public String callsign() { return state.data.callsign; }
+ public double time() { return state.data.time; }
+ public double rssi() { return state.data.rssi; }
- public int state() { return record.state; }
- public String state_name() { return record.state(); }
+ public int state() { return state.state; }
+ public String state_name() { return state.data.state(); }
- public double acceleration() { return record.acceleration(); }
- public double pressure() { return record.raw_pressure(); }
- public double altitude() { return record.raw_altitude(); }
- public double height() { return record.raw_height(); }
- public double accel_speed() { return record.accel_speed(); }
- public double baro_speed() { return state.baro_speed; }
- public double temperature() { return record.temperature(); }
- public double battery_voltage() { return record.battery_voltage(); }
- public double drogue_voltage() { return record.drogue_voltage(); }
- public double main_voltage() { return record.main_voltage(); }
- public boolean has_accel() { return has_accel; }
+ public double acceleration() { return state.acceleration; }
+ public double height() { return state.height; }
+ public double speed() { return state.speed(); }
+ public double temperature() { return state.temperature; }
+ public double battery_voltage() { return state.battery; }
+ public double drogue_voltage() { return state.drogue_sense; }
+ public double main_voltage() { return state.main_sense; }
+ public boolean has_accel() { return true; } // return state.acceleration != AltosRecord.MISSING; }
};
}
throw new UnsupportedOperationException();
}
public boolean hasNext() {
- if (record != null && record.state == Altos.ao_flight_landed)
+ if (state != null && state.state == Altos.ao_flight_landed)
return false;
return iter.hasNext();
}
public AltosDataPoint next() {
do {
read_next_record();
- } while (record.time < -1.0 && hasNext());
+ } while (state.data.time < -1.0 && hasNext());
return current_dp();
}
};
class Speed extends DescentValue {
void show (AltosState state, int crc_errors) {
- double speed = state.speed;
+ double speed = state.accel_speed;
if (!state.ascent)
speed = state.baro_speed;
show(AltosConvert.speed, speed);
if ((old_state == null || old_state.state <= Altos.ao_flight_boost) &&
state.state > Altos.ao_flight_boost) {
voice.speak("max speed: %s.",
- AltosConvert.speed.say_units(state.max_speed + 0.5));
+ AltosConvert.speed.say_units(state.max_accel_speed + 0.5));
ret = true;
} else if ((old_state == null || old_state.state < Altos.ao_flight_drogue) &&
state.state >= Altos.ao_flight_drogue) {
double max_height;
double max_speed;
double max_acceleration;
- double[] state_speed = new double[Altos.ao_flight_invalid + 1];
+ double[] state_accel_speed = new double[Altos.ao_flight_invalid + 1];
double[] state_baro_speed = new double[Altos.ao_flight_invalid + 1];
double[] state_accel = new double[Altos.ao_flight_invalid + 1];
int[] state_count = new int[Altos.ao_flight_invalid + 1];
}
}
state_accel[state.state] += state.acceleration;
- state_speed[state.state] += state.speed;
+ state_accel_speed[state.state] += state.accel_speed;
state_baro_speed[state.state] += state.baro_speed;
state_count[state.state]++;
if (state_start[state.state] == 0.0)
if (state_end[state.state] < state.time)
state_end[state.state] = state.time;
max_height = state.max_height;
- if (state.max_speed != 0)
- max_speed = state.max_speed;
+ if (state.max_accel_speed != 0)
+ max_speed = state.max_accel_speed;
else
max_speed = state.max_baro_speed;
max_acceleration = state.max_acceleration;
}
for (int s = Altos.ao_flight_startup; s <= Altos.ao_flight_landed; s++) {
if (state_count[s] > 0) {
- state_speed[s] /= state_count[s];
+ state_accel_speed[s] /= state_count[s];
state_baro_speed[s] /= state_count[s];
state_accel[s] /= state_count[s];
}
AltosGraphTime.Element speed =
new AltosGraphTime.TimeSeries(String.format("Speed (%s)", AltosConvert.speed.show_units()), "Vertical Speed", green) {
public void gotTimeData(double time, AltosDataPoint d) {
- double speed;
- if (d.state() < Altos.ao_flight_drogue && d.has_accel()) {
- speed = d.accel_speed();
- } else {
- speed = d.baro_speed();
- }
+ double speed = d.speed();
if (speed != AltosRecord.MISSING)
series.add(time, AltosConvert.speed.value(speed));
}
info_add_row(0, "Max height", "%6.0f m", state.max_height);
info_add_row(0, "Acceleration", "%8.1f m/s²", state.acceleration);
info_add_row(0, "Max acceleration", "%8.1f m/s²", state.max_acceleration);
- info_add_row(0, "Speed", "%8.1f m/s", state.ascent ? state.speed : state.baro_speed);
- info_add_row(0, "Max Speed", "%8.1f m/s", state.max_speed);
+ info_add_row(0, "Speed", "%8.1f m/s", state.speed());
+ info_add_row(0, "Max Speed", "%8.1f m/s", state.max_accel_speed);
info_add_row(0, "Temperature", "%9.2f °C", state.temperature);
info_add_row(0, "Battery", "%9.2f V", state.battery);
if (state.drogue_sense != AltosRecord.MISSING)
AltosGPS gps = record.gps;
out.printf(kml_coord_fmt,
gps.lon, gps.lat,
- record.filtered_altitude(), (double) gps.alt,
+ record.altitude(), (double) gps.alt,
record.time, gps.nsat);
}
class Speed extends LandedValue {
void show (AltosState state, int crc_errors) {
- show(AltosConvert.speed, state.max_speed);
+ show(AltosConvert.speed, state.max_speed());
}
public Speed (GridBagLayout layout, int y) {
super (layout, y, "Maximum Speed");