* General Public License for more details.
*/
-package org.altusmetrum.altoslib_11;
+package org.altusmetrum.altoslib_14;
/*
* Calibration and other data needed to construct 'real' values from various data
public int device_type = AltosLib.MISSING;
public void set_device_type(int device_type) {
- if (device_type != AltosLib.MISSING)
+ if (device_type != AltosLib.MISSING) {
this.device_type = device_type;
+ if (product == null)
+ set_product(AltosLib.product_name(device_type));
+ }
+ }
+
+ public int log_format = AltosLib.MISSING;
+
+ public void set_log_format(int log_format) {
+ if (log_format != AltosLib.MISSING)
+ this.log_format = log_format;
}
public int config_major = AltosLib.MISSING;
public void set_accel_plus_minus(double plus, double minus) {
if (plus != AltosLib.MISSING && minus != AltosLib.MISSING) {
+ if (plus == minus)
+ return;
accel_plus_g = plus;
accel_minus_g = minus;
}
this.ground_accel = ground_accel;
}
+ public double ground_motor_pressure = AltosLib.MISSING;
+
+ public void set_ground_motor_pressure(double ground_motor_pressure) {
+ if (ground_motor_pressure != AltosLib.MISSING)
+ this.ground_motor_pressure = ground_motor_pressure;
+ }
+
/* Raw acceleration value */
public double accel = AltosLib.MISSING;
mma655x_inverted = inverted;
}
+ public boolean adxl375_inverted = false;
+
+ public void set_adxl375_inverted(boolean inverted) {
+ adxl375_inverted = inverted;
+ }
+
+ public int adxl375_axis = AltosLib.MISSING;
+
+ public void set_adxl375_axis(int axis) {
+ adxl375_axis = axis;
+ }
+
public int pad_orientation = AltosLib.MISSING;
public void set_pad_orientation(int orientation) {
/* Compute acceleration */
public double acceleration(double sensor) {
- return AltosConvert.acceleration_from_sensor(sensor, accel_plus_g, accel_minus_g, ground_accel);
+ double accel;
+ accel = AltosConvert.acceleration_from_sensor(sensor, accel_plus_g, accel_minus_g, ground_accel);
+ return accel;
}
public AltosMs5607 ms5607 = null;
}
public int tick = AltosLib.MISSING;
+ private int first_tick = AltosLib.MISSING;
private int prev_tick = AltosLib.MISSING;
public void set_tick(int tick) {
tick += 65536;
}
}
+ if (first_tick == AltosLib.MISSING)
+ first_tick = tick;
+ prev_tick = tick;
this.tick = tick;
}
}
+ /* Reset all values which change during flight
+ */
+ public void reset() {
+ state = AltosLib.MISSING;
+ tick = AltosLib.MISSING;
+ prev_tick = AltosLib.MISSING;
+ temp_gps = null;
+ temp_gps_sat_tick = AltosLib.MISSING;
+ accel = AltosLib.MISSING;
+ }
+
public int boost_tick = AltosLib.MISSING;
public void set_boost_tick() {
public double time() {
if (tick == AltosLib.MISSING)
return AltosLib.MISSING;
- if (boost_tick == AltosLib.MISSING)
- return AltosLib.MISSING;
- return (tick - boost_tick) / ticks_per_sec;
+ if (boost_tick != AltosLib.MISSING)
+ return (tick - boost_tick) / ticks_per_sec;
+ if (first_tick != AltosLib.MISSING)
+ return (tick - first_tick) / ticks_per_sec;
+ return tick / ticks_per_sec;
}
public double boost_time() {
public int state = AltosLib.MISSING;
+ public String state_name() {
+ return AltosLib.state_name(state);
+ }
+
public void set_state(int state) {
if (state >= AltosLib.ao_flight_boost && boost_tick == AltosLib.MISSING)
set_boost_tick();
public AltosGPS gps_pad = null;
+ public AltosGPS prev_gps = null;
+
public double gps_pad_altitude = AltosLib.MISSING;
- public void set_gps(AltosGPS gps) {
- if ((state != AltosLib.MISSING && state < AltosLib.ao_flight_boost) || gps_pad == null)
- gps_pad = gps;
- if (gps_pad_altitude == AltosLib.MISSING && gps.alt != AltosLib.MISSING)
- gps_pad_altitude = gps.alt;
+ public void set_cal_gps(AltosGPS gps) {
+ if (gps.locked && gps.nsat >= 4) {
+ if ((state != AltosLib.MISSING && state < AltosLib.ao_flight_boost) || gps_pad == null)
+ gps_pad = gps;
+ if (gps_pad_altitude == AltosLib.MISSING && gps.alt != AltosLib.MISSING)
+ gps_pad_altitude = gps.alt;
+ }
+ temp_gps = null;
+ prev_gps = gps;
}
/*
* object and then deliver the result atomically to the listener
*/
AltosGPS temp_gps = null;
- AltosGPS prev_gps = null;
int temp_gps_sat_tick = AltosLib.MISSING;
- public AltosGPS temp_gps() {
+ public AltosGPS temp_cal_gps() {
return temp_gps;
}
- public void reset_temp_gps() {
- if (temp_gps != null) {
- if (temp_gps.locked && temp_gps.nsat >= 4)
- set_gps(temp_gps);
- prev_gps = temp_gps;
- temp_gps = null;
- }
+ public void reset_temp_cal_gps() {
+ if (temp_gps != null)
+ set_cal_gps(temp_gps);
}
- public boolean gps_pending() {
+ public boolean cal_gps_pending() {
return temp_gps != null;
}
- public AltosGPS make_temp_gps(int tick, boolean sats) {
- if (temp_gps == null) {
- if (prev_gps != null)
- temp_gps = prev_gps.clone();
- else
- temp_gps = new AltosGPS();
- }
+ public AltosGPS make_temp_cal_gps(int tick, boolean sats) {
+ if (temp_gps == null)
+ temp_gps = new AltosGPS(prev_gps);
if (sats) {
if (tick != temp_gps_sat_tick)
temp_gps.cc_gps_sat = null;
return temp_gps;
}
+ public int imu_type = AltosLib.MISSING;
+
+ public int imu_model = AltosLib.MISSING;
+
+ public int mag_model = AltosLib.MISSING;
+
+ public void set_imu_type(int imu_type) {
+ this.imu_type = imu_type;
+ }
+
+ public void set_imu_model(int imu_model) {
+ this.imu_model = imu_model;
+ }
+
+ public void set_mag_model(int mag_model) {
+ this.mag_model = mag_model;
+ }
+
public double accel_zero_along, accel_zero_across, accel_zero_through;
public void set_accel_zero(double zero_along, double zero_across, double zero_through) {
}
public double accel_along(double counts) {
- return AltosIMU.convert_accel(counts - accel_zero_along);
+ return AltosIMU.convert_accel(counts - accel_zero_along, imu_type, imu_model);
}
public double accel_across(double counts) {
- return AltosIMU.convert_accel(counts - accel_zero_across);
+ return AltosIMU.convert_accel(counts - accel_zero_across, imu_type, imu_model);
}
public double accel_through(double counts) {
- return AltosIMU.convert_accel(counts - accel_zero_through);
+ return AltosIMU.convert_accel(counts - accel_zero_through, imu_type, imu_model);
}
- public double gyro_zero_roll, gyro_zero_pitch, gyro_zero_yaw;
+ public double gyro_zero_roll = AltosLib.MISSING;
+ public double gyro_zero_pitch = AltosLib.MISSING;
+ public double gyro_zero_yaw = AltosLib.MISSING;
public void set_gyro_zero(double roll, double pitch, double yaw) {
if (roll != AltosLib.MISSING) {
gyro_zero_roll = roll;
gyro_zero_pitch = pitch;
gyro_zero_yaw = yaw;
+ imu_wrap_checked = false;
}
}
public double gyro_roll(double counts) {
if (gyro_zero_roll == AltosLib.MISSING || counts == AltosLib.MISSING)
return AltosLib.MISSING;
- return AltosIMU.convert_gyro(counts - gyro_zero_roll);
+
+ return AltosIMU.gyro_degrees_per_second(counts - gyro_zero_roll, imu_type, imu_model);
}
public double gyro_pitch(double counts) {
if (gyro_zero_pitch == AltosLib.MISSING || counts == AltosLib.MISSING)
return AltosLib.MISSING;
- return AltosIMU.convert_gyro(counts - gyro_zero_pitch);
+ return AltosIMU.gyro_degrees_per_second(counts - gyro_zero_pitch, imu_type, imu_model);
}
public double gyro_yaw(double counts) {
if (gyro_zero_yaw == AltosLib.MISSING || counts == AltosLib.MISSING)
return AltosLib.MISSING;
- return AltosIMU.convert_gyro(counts - gyro_zero_yaw);
+ return AltosIMU.gyro_degrees_per_second(counts - gyro_zero_yaw, imu_type, imu_model);
}
private double gyro_zero_overflow(double first) {
v = Math.ceil(v);
else
v = Math.floor(v);
+// if (v != 0)
+// System.out.printf("Adjusting gyro axis by %g steps\n", v);
return v * 128.0;
}
+ /* Initial TeleMega log format had only 16 bits for gyro cal, so the top 9 bits got lost as the
+ * cal data are scaled by 512. Use the first sample to adjust the cal value, assuming that it is
+ * from a time of fairly low rotation speed. Fixed in later TeleMega firmware by storing 32 bits
+ * of cal values.
+ */
+ private boolean imu_wrap_checked = false;
+
public void check_imu_wrap(double roll, double pitch, double yaw) {
- gyro_zero_roll += gyro_zero_overflow(roll);
- gyro_zero_pitch += gyro_zero_overflow(pitch);
- gyro_zero_yaw += gyro_zero_overflow(yaw);
+ if (!imu_wrap_checked) {
+ gyro_zero_roll += gyro_zero_overflow(roll);
+ gyro_zero_pitch += gyro_zero_overflow(pitch);
+ gyro_zero_yaw += gyro_zero_overflow(yaw);
+ imu_wrap_checked = true;
+ }
}
public double mag_along(double along) {
if (along == AltosLib.MISSING)
return AltosLib.MISSING;
- return AltosMag.convert_gauss(along);
+ return AltosMag.convert_gauss(along, imu_type, mag_model);
}
public double mag_across(double across) {
if (across == AltosLib.MISSING)
return AltosLib.MISSING;
- return AltosMag.convert_gauss(across);
+ return AltosMag.convert_gauss(across, imu_type, mag_model);
}
public double mag_through(double through) {
if (through == AltosLib.MISSING)
return AltosLib.MISSING;
- return AltosMag.convert_gauss(through);
+ return AltosMag.convert_gauss(through, imu_type, mag_model);
}
public AltosCalData() {
public AltosCalData(AltosConfigData config_data) {
set_serial(config_data.serial);
+ set_ticks_per_sec(100.0);
set_flight(config_data.flight);
set_callsign(config_data.callsign);
- set_accel_plus_minus(config_data.accel_cal_plus, config_data.accel_cal_minus);
+ set_config(config_data.config_major, config_data.config_minor, config_data.flight_log_max);
+ set_firmware_version(config_data.version);
+ set_flight_params(config_data.apogee_delay / ticks_per_sec, config_data.apogee_lockout / ticks_per_sec);
+ set_pad_orientation(config_data.pad_orientation);
+ set_product(config_data.product);
+ set_accel_plus_minus(config_data.accel_cal_plus(config_data.pad_orientation), config_data.accel_cal_minus(config_data.pad_orientation));
+ set_accel_zero(config_data.accel_zero_along, config_data.accel_zero_across, config_data.accel_zero_through);
set_ms5607(config_data.ms5607);
try {
set_mma655x_inverted(config_data.mma655x_inverted());
} catch (AltosUnknownProduct up) {
}
+ try {
+ set_adxl375_inverted(config_data.adxl375_inverted());
+ } catch (AltosUnknownProduct up) {
+ }
+ try {
+ set_adxl375_axis(config_data.adxl375_axis());
+ } catch (AltosUnknownProduct up) {
+ }
set_pad_orientation(config_data.pad_orientation);
}
}