X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosFlightSeries.java;h=315e0a44d6fbd39924d5168acd9df53f844dc1d0;hp=09b537758540ecbad4b9e6577bf28b954bc7a879;hb=af6ce9a142d001f3b9b83cf6527f4dc848e94d48;hpb=1b91ba98395a431385c9af5de77b2892fd6ccc43 diff --git a/altoslib/AltosFlightSeries.java b/altoslib/AltosFlightSeries.java index 09b53775..315e0a44 100644 --- a/altoslib/AltosFlightSeries.java +++ b/altoslib/AltosFlightSeries.java @@ -143,6 +143,10 @@ public class AltosFlightSeries extends AltosDataListener { public static final String state_name = "State"; public void set_state(int state) { + + if (state == AltosLib.ao_flight_pad) + return; + if (state_series == null) state_series = add_series(state_name, AltosConvert.state_name); else if (this.state == state) @@ -155,10 +159,16 @@ public class AltosFlightSeries extends AltosDataListener { public static final String accel_name = "Accel"; + public AltosTimeSeries vert_accel_series; + + public static final String vert_accel_name = "Vertical Accel"; + public void set_acceleration(double acceleration) { - if (accel_series == null) { + if (acceleration == AltosLib.MISSING) + return; + if (accel_series == null) accel_series = add_series(accel_name, AltosConvert.accel); - } + accel_series.add(time(), acceleration); } @@ -207,6 +217,9 @@ public class AltosFlightSeries extends AltosDataListener { public static final String height_name = "Height"; public void set_pressure(double pa) { + if (pa == AltosLib.MISSING) + return; + if (pressure_series == null) pressure_series = add_series(pressure_name, AltosConvert.pressure); pressure_series.add(time(), pa); @@ -227,6 +240,13 @@ public class AltosFlightSeries extends AltosDataListener { for (AltosTimeValue alt : altitude_series) height_series.add(alt.time, alt.value - ground_altitude); } + + if (gps_height == null && cal_data.gps_pad != null && cal_data.gps_pad.alt != AltosLib.MISSING && gps_altitude != null) { + double gps_ground_altitude = cal_data.gps_pad.alt; + gps_height = add_series(gps_height_name, AltosConvert.height); + for (AltosTimeValue gps_alt : gps_altitude) + gps_height.add(gps_alt.time, gps_alt.value - gps_ground_altitude); + } } public AltosTimeSeries speed_series; @@ -248,8 +268,25 @@ public class AltosFlightSeries extends AltosDataListener { temp_series.differentiate(alt_speed_series); } if (accel_series != null) { + + if (orient_series != null) { + vert_accel_series = add_series(vert_accel_name, AltosConvert.accel); + + for (AltosTimeValue a : accel_series) { + double orient = orient_series.value(a.time); + double a_abs = a.value + AltosConvert.gravity; + double v_a = a_abs * Math.cos(AltosConvert.degrees_to_radians(orient)) - AltosConvert.gravity; + + vert_accel_series.add(a.time, v_a); + } + } + AltosTimeSeries temp_series = make_series(speed_name, AltosConvert.speed); - accel_series.integrate(temp_series); + + if (vert_accel_series != null) + vert_accel_series.integrate(temp_series); + else + accel_series.integrate(temp_series); accel_speed_series = make_series(speed_name, AltosConvert.speed); temp_series.filter(accel_speed_series, 0.1); @@ -288,6 +325,49 @@ public class AltosFlightSeries extends AltosDataListener { add_series(speed_series); } + public AltosTimeSeries orient_series; + + public static final String orient_name = "Tilt Angle"; + + private void compute_orient() { + + if (orient_series != null) + return; + + if (accel_ground_across == AltosLib.MISSING) + return; + + if (cal_data.pad_orientation == AltosLib.MISSING) + return; + + if (cal_data.accel_zero_across == AltosLib.MISSING) + return; + + AltosRotation 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); + double prev_time = ground_time; + + orient_series = add_series(orient_name, AltosConvert.orient); + orient_series.add(ground_time, rotation.tilt()); + + for (AltosTimeValue roll_v : gyro_roll) { + double time = roll_v.time; + double dt = time - prev_time; + + if (dt > 0) { + double roll = AltosConvert.degrees_to_radians(roll_v.value); + double pitch = AltosConvert.degrees_to_radians(gyro_pitch.value(time)); + double yaw = AltosConvert.degrees_to_radians(gyro_yaw.value(time)); + + rotation.rotate(dt, pitch, yaw, roll); + orient_series.add(time, rotation.tilt()); + } + prev_time = time; + } + } + public AltosTimeSeries kalman_height_series, kalman_speed_series, kalman_accel_series; public static final String kalman_height_name = "Kalman Height"; @@ -420,28 +500,32 @@ public class AltosFlightSeries extends AltosDataListener { gps_vdop.add(time(), gps.vdop); } if (gps.locked) { - if (gps_altitude == null) { - gps_altitude = add_series(gps_altitude_name, AltosConvert.height); - gps_height = add_series(gps_height_name, AltosConvert.height); - gps_ground_speed = add_series(gps_ground_speed_name, AltosConvert.speed); - gps_ascent_rate = add_series(gps_ascent_rate_name, AltosConvert.speed); - gps_course = add_series(gps_course_name, null); - gps_speed = add_series(gps_speed_name, null); - } if (gps.alt != AltosLib.MISSING) { + if (gps_altitude == null) + gps_altitude = add_series(gps_altitude_name, AltosConvert.height); gps_altitude.add(time(), gps.alt); - if (cal_data.gps_pad != null) - gps_height.add(time(), gps.alt - cal_data.gps_pad.alt); } - if (gps.ground_speed != AltosLib.MISSING) + if (gps.ground_speed != AltosLib.MISSING) { + if (gps_ground_speed == null) + gps_ground_speed = add_series(gps_ground_speed_name, AltosConvert.speed); gps_ground_speed.add(time(), gps.ground_speed); - if (gps.climb_rate != AltosLib.MISSING) + } + if (gps.climb_rate != AltosLib.MISSING) { + if (gps_ascent_rate == null) + gps_ascent_rate = add_series(gps_ascent_rate_name, AltosConvert.speed); gps_ascent_rate.add(time(), gps.climb_rate); - if (gps.course != AltosLib.MISSING) + } + if (gps.course != AltosLib.MISSING) { + if (gps_course == null) + gps_course = add_series(gps_course_name, null); gps_course.add(time(), gps.course); - if (gps.ground_speed != AltosLib.MISSING && gps.climb_rate != AltosLib.MISSING) + } + if (gps.ground_speed != AltosLib.MISSING && gps.climb_rate != AltosLib.MISSING) { + if (gps_speed == null) + gps_speed = add_series(gps_speed_name, null); gps_speed.add(time(), Math.sqrt(gps.ground_speed * gps.ground_speed + gps.climb_rate * gps.climb_rate)); + } } if (gps.cc_gps_sat != null) { if (sats_in_view == null) @@ -479,7 +563,17 @@ public class AltosFlightSeries extends AltosDataListener { accel_through.add(time(), through); } + private double accel_ground_along = AltosLib.MISSING; + private double accel_ground_across = AltosLib.MISSING; + private double accel_ground_through = AltosLib.MISSING; + + private double ground_time; + public void set_accel_ground(double along, double across, double through) { + accel_ground_along = along; + accel_ground_across = across; + accel_ground_through = through; + ground_time = time(); } public void set_gyro(double roll, double pitch, double yaw) { @@ -504,10 +598,6 @@ public class AltosFlightSeries extends AltosDataListener { mag_through.add(time(), through); } - public static final String orient_name = "Tilt Angle"; - - public AltosTimeSeries orient_series; - public void set_orient(double orient) { if (orient_series == null) orient_series = add_series(orient_name, AltosConvert.orient); @@ -584,6 +674,7 @@ public class AltosFlightSeries extends AltosDataListener { } public void finish() { + compute_orient(); compute_speed(); compute_accel(); compute_height();