2 * Copyright © 2017 Keith Packard <keithp@keithp.com>
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful, but
10 * WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 * General Public License for more details.
15 package org.altusmetrum.altoslib_14;
18 * Calibration and other data needed to construct 'real' values from various data
22 public class AltosCalData {
23 public int flight = AltosLib.MISSING;
25 public void set_flight(int flight) {
26 if (flight != AltosLib.MISSING)
30 public String callsign = null;
32 public void set_callsign(String callsign) {
34 this.callsign = callsign;
37 public String firmware_version = null;
39 public void set_firmware_version(String firmware_version) {
40 if (firmware_version != null)
41 this.firmware_version = firmware_version;
44 public String product = null;
46 public void set_product(String product) {
48 this.product = product;
51 public int serial = AltosLib.MISSING;
53 public void set_serial(int serial) {
54 if (serial != AltosLib.MISSING)
58 public int receiver_serial = AltosLib.MISSING;
60 public void set_receiver_serial(int receiver_serial) {
61 if (receiver_serial != AltosLib.MISSING)
62 this.receiver_serial = receiver_serial;
65 public int device_type = AltosLib.MISSING;
67 public void set_device_type(int device_type) {
68 if (device_type != AltosLib.MISSING) {
69 this.device_type = device_type;
71 set_product(AltosLib.product_name(device_type));
75 public int log_format = AltosLib.MISSING;
77 public void set_log_format(int log_format) {
78 if (log_format != AltosLib.MISSING)
79 this.log_format = log_format;
82 public int config_major = AltosLib.MISSING;
83 public int config_minor = AltosLib.MISSING;
84 public int flight_log_max = AltosLib.MISSING;
86 public void set_config(int major, int minor, int log_max) {
87 if (major != AltosLib.MISSING)
89 if (minor != AltosLib.MISSING)
91 if (log_max != AltosLib.MISSING)
92 flight_log_max = log_max;
95 public double apogee_delay = AltosLib.MISSING;
96 public double main_deploy = AltosLib.MISSING;
98 public void set_flight_params(double apogee_delay, double main_deploy) {
99 if (apogee_delay != AltosLib.MISSING)
100 this.apogee_delay = apogee_delay;
101 if (main_deploy != AltosLib.MISSING)
102 this.main_deploy = main_deploy;
105 public double accel_plus_g = AltosLib.MISSING;
106 public double accel_minus_g = AltosLib.MISSING;
107 public double ground_accel = AltosLib.MISSING;
109 public void set_accel_plus_minus(double plus, double minus) {
110 if (plus != AltosLib.MISSING && minus != AltosLib.MISSING) {
114 accel_minus_g = minus;
118 public void set_ground_accel(double ground_accel) {
119 if (ground_accel != AltosLib.MISSING)
120 this.ground_accel = ground_accel;
123 /* Raw acceleration value */
124 public double accel = AltosLib.MISSING;
126 public void set_accel(double accel) {
130 public boolean mma655x_inverted = false;
132 public void set_mma655x_inverted(boolean inverted) {
133 mma655x_inverted = inverted;
136 public boolean adxl375_inverted = false;
138 public void set_adxl375_inverted(boolean inverted) {
139 adxl375_inverted = inverted;
142 public int adxl375_axis = AltosLib.MISSING;
144 public void set_adxl375_axis(int axis) {
148 public int pad_orientation = AltosLib.MISSING;
150 public void set_pad_orientation(int orientation) {
151 if (orientation != AltosLib.MISSING)
152 pad_orientation = orientation;
155 /* Compute acceleration */
156 public double acceleration(double sensor) {
158 accel = AltosConvert.acceleration_from_sensor(sensor, accel_plus_g, accel_minus_g, ground_accel);
162 public AltosMs5607 ms5607 = null;
164 public void set_ms5607(AltosMs5607 ms5607) {
165 this.ms5607 = ms5607;
168 public double ground_pressure = AltosLib.MISSING;
169 public double ground_altitude = AltosLib.MISSING;
171 public void set_ground_pressure(double ground_pressure) {
172 if (ground_pressure != AltosLib.MISSING) {
173 this.ground_pressure = ground_pressure;
174 this.ground_altitude = AltosConvert.pressure_to_altitude(ground_pressure);
178 public void set_ground_altitude(double ground_altitude) {
179 if (ground_altitude != AltosLib.MISSING)
180 this.ground_altitude = ground_altitude;
183 /* Compute pressure */
185 public AltosPresTemp pressure_ms5607(int raw_pres, int raw_temp) {
187 return new AltosPresTemp(AltosLib.MISSING, AltosLib.MISSING);
188 return ms5607.pres_temp(raw_pres, raw_temp);
191 public int tick = AltosLib.MISSING;
192 private int first_tick = AltosLib.MISSING;
193 private int prev_tick = AltosLib.MISSING;
195 public void set_tick(int tick) {
196 if (tick != AltosLib.MISSING) {
197 if (prev_tick != AltosLib.MISSING) {
198 while (tick < prev_tick - 1000) {
202 if (first_tick == AltosLib.MISSING)
209 /* Reset all values which change during flight
211 public void reset() {
212 state = AltosLib.MISSING;
213 tick = AltosLib.MISSING;
214 prev_tick = AltosLib.MISSING;
216 temp_gps_sat_tick = AltosLib.MISSING;
217 accel = AltosLib.MISSING;
220 public int boost_tick = AltosLib.MISSING;
222 public void set_boost_tick() {
226 public double ticks_per_sec = 100.0;
228 public void set_ticks_per_sec(double ticks_per_sec) {
229 this.ticks_per_sec = ticks_per_sec;
232 public double time() {
233 if (tick == AltosLib.MISSING)
234 return AltosLib.MISSING;
235 if (boost_tick != AltosLib.MISSING)
236 return (tick - boost_tick) / ticks_per_sec;
237 if (first_tick != AltosLib.MISSING)
238 return (tick - first_tick) / ticks_per_sec;
239 return tick / ticks_per_sec;
242 public double boost_time() {
243 if (boost_tick == AltosLib.MISSING)
244 return AltosLib.MISSING;
245 return boost_tick / ticks_per_sec;
248 public int state = AltosLib.MISSING;
250 public String state_name() {
251 return AltosLib.state_name(state);
254 public void set_state(int state) {
255 if (state >= AltosLib.ao_flight_boost && boost_tick == AltosLib.MISSING)
260 public AltosGPS gps_pad = null;
262 public AltosGPS prev_gps = null;
264 public double gps_pad_altitude = AltosLib.MISSING;
266 public void set_cal_gps(AltosGPS gps) {
267 if (gps.locked && gps.nsat >= 4) {
268 if ((state != AltosLib.MISSING && state < AltosLib.ao_flight_boost) || gps_pad == null)
270 if (gps_pad_altitude == AltosLib.MISSING && gps.alt != AltosLib.MISSING)
271 gps_pad_altitude = gps.alt;
278 * While receiving GPS data, we construct a temporary GPS state
279 * object and then deliver the result atomically to the listener
281 AltosGPS temp_gps = null;
282 int temp_gps_sat_tick = AltosLib.MISSING;
284 public AltosGPS temp_cal_gps() {
288 public void reset_temp_cal_gps() {
289 if (temp_gps != null)
290 set_cal_gps(temp_gps);
293 public boolean cal_gps_pending() {
294 return temp_gps != null;
297 public AltosGPS make_temp_cal_gps(int tick, boolean sats) {
298 if (temp_gps == null)
299 temp_gps = new AltosGPS(prev_gps);
301 if (tick != temp_gps_sat_tick)
302 temp_gps.cc_gps_sat = null;
303 temp_gps_sat_tick = tick;
308 public int imu_type = AltosLib.MISSING;;
310 public void set_imu_type(int imu_type) {
311 this.imu_type = imu_type;
314 public double accel_zero_along, accel_zero_across, accel_zero_through;
316 public void set_accel_zero(double zero_along, double zero_across, double zero_through) {
317 if (zero_along != AltosLib.MISSING) {
318 accel_zero_along = zero_along;
319 accel_zero_across = zero_across;
320 accel_zero_through = zero_through;
324 public double accel_along(double counts) {
325 return AltosIMU.convert_accel(counts - accel_zero_along, imu_type);
328 public double accel_across(double counts) {
329 return AltosIMU.convert_accel(counts - accel_zero_across, imu_type);
332 public double accel_through(double counts) {
333 return AltosIMU.convert_accel(counts - accel_zero_through, imu_type);
336 public double gyro_zero_roll, gyro_zero_pitch, gyro_zero_yaw;
338 public void set_gyro_zero(double roll, double pitch, double yaw) {
339 if (roll != AltosLib.MISSING) {
340 gyro_zero_roll = roll;
341 gyro_zero_pitch = pitch;
343 imu_wrap_checked = false;
347 public double gyro_roll(double counts) {
348 if (gyro_zero_roll == AltosLib.MISSING || counts == AltosLib.MISSING)
349 return AltosLib.MISSING;
351 return AltosIMU.gyro_degrees_per_second(counts - gyro_zero_roll, imu_type);
354 public double gyro_pitch(double counts) {
355 if (gyro_zero_pitch == AltosLib.MISSING || counts == AltosLib.MISSING)
356 return AltosLib.MISSING;
357 return AltosIMU.gyro_degrees_per_second(counts - gyro_zero_pitch, imu_type);
360 public double gyro_yaw(double counts) {
361 if (gyro_zero_yaw == AltosLib.MISSING || counts == AltosLib.MISSING)
362 return AltosLib.MISSING;
363 return AltosIMU.gyro_degrees_per_second(counts - gyro_zero_yaw, imu_type);
366 private double gyro_zero_overflow(double first) {
367 double v = first / 128.0;
373 // System.out.printf("Adjusting gyro axis by %g steps\n", v);
377 /* Initial TeleMega log format had only 16 bits for gyro cal, so the top 9 bits got lost as the
378 * cal data are scaled by 512. Use the first sample to adjust the cal value, assuming that it is
379 * from a time of fairly low rotation speed. Fixed in later TeleMega firmware by storing 32 bits
382 private boolean imu_wrap_checked = false;
384 public void check_imu_wrap(double roll, double pitch, double yaw) {
385 if (!imu_wrap_checked) {
386 gyro_zero_roll += gyro_zero_overflow(roll);
387 gyro_zero_pitch += gyro_zero_overflow(pitch);
388 gyro_zero_yaw += gyro_zero_overflow(yaw);
389 imu_wrap_checked = true;
393 public double mag_along(double along) {
394 if (along == AltosLib.MISSING)
395 return AltosLib.MISSING;
396 return AltosIMU.convert_gauss(along, imu_type, AltosIMU.mag_along_axis(imu_type));
399 public double mag_across(double across) {
400 if (across == AltosLib.MISSING)
401 return AltosLib.MISSING;
402 return AltosIMU.convert_gauss(across, imu_type, AltosIMU.mag_across_axis(imu_type));
405 public double mag_through(double through) {
406 if (through == AltosLib.MISSING)
407 return AltosLib.MISSING;
408 return AltosIMU.convert_gauss(through, imu_type, AltosIMU.mag_through_axis(imu_type));
411 public AltosCalData() {
414 public AltosCalData(AltosConfigData config_data) {
415 set_serial(config_data.serial);
416 set_ticks_per_sec(100.0);
417 set_flight(config_data.flight);
418 set_callsign(config_data.callsign);
419 set_config(config_data.config_major, config_data.config_minor, config_data.flight_log_max);
420 set_firmware_version(config_data.version);
421 set_flight_params(config_data.apogee_delay / ticks_per_sec, config_data.apogee_lockout / ticks_per_sec);
422 set_pad_orientation(config_data.pad_orientation);
423 set_product(config_data.product);
424 set_accel_plus_minus(config_data.accel_cal_plus(config_data.pad_orientation), config_data.accel_cal_minus(config_data.pad_orientation));
425 set_accel_zero(config_data.accel_zero_along, config_data.accel_zero_across, config_data.accel_zero_through);
426 set_ms5607(config_data.ms5607);
428 set_mma655x_inverted(config_data.mma655x_inverted());
429 } catch (AltosUnknownProduct up) {
432 set_adxl375_inverted(config_data.adxl375_inverted());
433 } catch (AltosUnknownProduct up) {
436 set_adxl375_axis(config_data.adxl375_axis());
437 } catch (AltosUnknownProduct up) {
439 set_pad_orientation(config_data.pad_orientation);