* Track flight state from telemetry or eeprom data stream
*/
-package org.altusmetrum.altoslib_11;
-
-import java.io.*;
+package org.altusmetrum.altoslib_12;
public class AltosState extends AltosDataListener {
public AltosGPS gps;
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;
gps = null;
gps_pending = false;
- imu = null;
last_imu_time = AltosLib.MISSING;
rotation = null;
- ground_rotation = null;
-
- mag = null;
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;
void update_gps() {
elevation = AltosLib.MISSING;
+ distance = AltosLib.MISSING;
range = AltosLib.MISSING;
if (gps == null)
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 AltosRotation rotation;
- public AltosRotation ground_rotation;
-
- 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 - 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),
- 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 AltosState() {
+ Thread.dumpStack();
init();
}