git.gag.com Git - fw/altos/blob - altoslib/AltosCalData.java

   1 /*
   2  * Copyright © 2017 Keith Packard <keithp@keithp.com>
   3  *
   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.
   8  *
   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.
  13  */
  14
  15 package org.altusmetrum.altoslib_14;
  16
  17 /*
  18  * Calibration and other data needed to construct 'real' values from various data
  19  * sources.
  20  */
  21
  22 public class AltosCalData {
  23         public int              flight = AltosLib.MISSING;
  24
  25         public void set_flight(int flight) {
  26                 if (flight != AltosLib.MISSING)
  27                         this.flight = flight;
  28         }
  29
  30         public String           callsign = null;
  31
  32         public void set_callsign(String callsign) {
  33                 if (callsign != null)
  34                         this.callsign = callsign;
  35         }
  36
  37         public String           firmware_version = null;
  38
  39         public void set_firmware_version(String firmware_version) {
  40                 if (firmware_version != null)
  41                         this.firmware_version = firmware_version;
  42         }
  43
  44         public String           product = null;
  45
  46         public void set_product(String product) {
  47                 if (product != null)
  48                         this.product = product;
  49         }
  50
  51         public int              serial = AltosLib.MISSING;
  52
  53         public void set_serial(int serial) {
  54                 if (serial != AltosLib.MISSING)
  55                         this.serial = serial;
  56         }
  57
  58         public int              receiver_serial = AltosLib.MISSING;
  59
  60         public void set_receiver_serial(int receiver_serial) {
  61                 if (receiver_serial != AltosLib.MISSING)
  62                         this.receiver_serial = receiver_serial;
  63         }
  64
  65         public int              device_type = AltosLib.MISSING;
  66
  67         public void set_device_type(int device_type) {
  68                 if (device_type != AltosLib.MISSING) {
  69                         this.device_type = device_type;
  70                         if (product == null)
  71                                 set_product(AltosLib.product_name(device_type));
  72                 }
  73         }
  74
  75         public int              log_format = AltosLib.MISSING;
  76
  77         public void set_log_format(int log_format) {
  78                 if (log_format != AltosLib.MISSING)
  79                         this.log_format = log_format;
  80         }
  81
  82         public int              config_major = AltosLib.MISSING;
  83         public int              config_minor = AltosLib.MISSING;
  84         public int              flight_log_max = AltosLib.MISSING;
  85
  86         public void set_config(int major, int minor, int log_max) {
  87                 if (major != AltosLib.MISSING)
  88                         config_major = major;
  89                 if (minor != AltosLib.MISSING)
  90                         config_minor = minor;
  91                 if (log_max != AltosLib.MISSING)
  92                         flight_log_max = log_max;
  93         }
  94
  95         public double           apogee_delay = AltosLib.MISSING;
  96         public double           main_deploy = AltosLib.MISSING;
  97
  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;
 103         }
 104
 105         public double           accel_plus_g = AltosLib.MISSING;
 106         public double           accel_minus_g = AltosLib.MISSING;
 107         public double           ground_accel = AltosLib.MISSING;
 108
 109         public void set_accel_plus_minus(double plus, double minus) {
 110                 if (plus != AltosLib.MISSING && minus != AltosLib.MISSING) {
 111                         if (plus == minus)
 112                                 return;
 113                         accel_plus_g = plus;
 114                         accel_minus_g = minus;
 115                 }
 116         }
 117
 118         public void set_ground_accel(double ground_accel) {
 119                 if (ground_accel != AltosLib.MISSING)
 120                         this.ground_accel = ground_accel;
 121         }
 122
 123         public double           ground_motor_pressure = AltosLib.MISSING;
 124
 125         public void set_ground_motor_pressure(double ground_motor_pressure) {
 126                 if (ground_motor_pressure != AltosLib.MISSING)
 127                         this.ground_motor_pressure = ground_motor_pressure;
 128         }
 129
 130         /* Raw acceleration value */
 131         public double           accel = AltosLib.MISSING;
 132
 133         public void set_accel(double accel) {
 134                 this.accel = accel;
 135         }
 136
 137         public boolean mma655x_inverted = false;
 138
 139         public void set_mma655x_inverted(boolean inverted) {
 140                 mma655x_inverted = inverted;
 141         }
 142
 143         public boolean adxl375_inverted = false;
 144
 145         public void set_adxl375_inverted(boolean inverted) {
 146                 adxl375_inverted = inverted;
 147         }
 148
 149         public int adxl375_axis = AltosLib.MISSING;
 150
 151         public void set_adxl375_axis(int axis) {
 152                 adxl375_axis = axis;
 153         }
 154
 155         public int pad_orientation = AltosLib.MISSING;
 156
 157         public void set_pad_orientation(int orientation) {
 158                 if (orientation != AltosLib.MISSING)
 159                         pad_orientation = orientation;
 160         }
 161
 162         /* Compute acceleration */
 163         public double acceleration(double sensor) {
 164                 double accel;
 165                 accel = AltosConvert.acceleration_from_sensor(sensor, accel_plus_g, accel_minus_g, ground_accel);
 166                 return accel;
 167         }
 168
 169         public AltosMs5607      ms5607 = null;
 170
 171         public void set_ms5607(AltosMs5607 ms5607) {
 172                 this.ms5607 = ms5607;
 173         }
 174
 175         public double           ground_pressure = AltosLib.MISSING;
 176         public double           ground_altitude = AltosLib.MISSING;
 177
 178         public void set_ground_pressure(double ground_pressure) {
 179                 if (ground_pressure != AltosLib.MISSING) {
 180                         this.ground_pressure = ground_pressure;
 181                         this.ground_altitude = AltosConvert.pressure_to_altitude(ground_pressure);
 182                 }
 183         }
 184
 185         public void set_ground_altitude(double ground_altitude) {
 186                 if (ground_altitude != AltosLib.MISSING)
 187                         this.ground_altitude = ground_altitude;
 188         }
 189
 190         /* Compute pressure */
 191
 192         public AltosPresTemp pressure_ms5607(int raw_pres, int raw_temp) {
 193                 if (ms5607 == null)
 194                         return new AltosPresTemp(AltosLib.MISSING, AltosLib.MISSING);
 195                 return ms5607.pres_temp(raw_pres, raw_temp);
 196         }
 197
 198         public int              tick = AltosLib.MISSING;
 199         private int             first_tick = AltosLib.MISSING;
 200         private int             prev_tick = AltosLib.MISSING;
 201
 202         public void set_tick(int tick) {
 203                 if (tick != AltosLib.MISSING) {
 204                         if (prev_tick != AltosLib.MISSING) {
 205                                 while (tick < prev_tick - 1000) {
 206                                         tick += 65536;
 207                                 }
 208                         }
 209                         if (first_tick == AltosLib.MISSING)
 210                                 first_tick = tick;
 211                         prev_tick = tick;
 212                         this.tick = tick;
 213                 }
 214         }
 215
 216         /* Reset all values which change during flight
 217          */
 218         public void reset() {
 219                 state = AltosLib.MISSING;
 220                 tick = AltosLib.MISSING;
 221                 prev_tick = AltosLib.MISSING;
 222                 temp_gps = null;
 223                 temp_gps_sat_tick = AltosLib.MISSING;
 224                 accel = AltosLib.MISSING;
 225         }
 226
 227         public int              boost_tick = AltosLib.MISSING;
 228
 229         public void set_boost_tick() {
 230                 boost_tick = tick;
 231         }
 232
 233         public double           ticks_per_sec = 100.0;
 234
 235         public void set_ticks_per_sec(double ticks_per_sec) {
 236                 this.ticks_per_sec = ticks_per_sec;
 237         }
 238
 239         public double time() {
 240                 if (tick == AltosLib.MISSING)
 241                         return AltosLib.MISSING;
 242                 if (boost_tick != AltosLib.MISSING)
 243                         return (tick - boost_tick) / ticks_per_sec;
 244                 if (first_tick != AltosLib.MISSING)
 245                         return (tick - first_tick) / ticks_per_sec;
 246                 return tick / ticks_per_sec;
 247         }
 248
 249         public double boost_time() {
 250                 if (boost_tick == AltosLib.MISSING)
 251                         return AltosLib.MISSING;
 252                 return boost_tick / ticks_per_sec;
 253         }
 254
 255         public int              state = AltosLib.MISSING;
 256
 257         public String state_name() {
 258                 return AltosLib.state_name(state);
 259         }
 260
 261         public void set_state(int state) {
 262                 if (state >= AltosLib.ao_flight_boost && boost_tick == AltosLib.MISSING)
 263                         set_boost_tick();
 264                 this.state = state;
 265         }
 266
 267         public AltosGPS         gps_pad = null;
 268
 269         public AltosGPS         prev_gps = null;
 270
 271         public double           gps_pad_altitude = AltosLib.MISSING;
 272
 273         public void set_cal_gps(AltosGPS gps) {
 274                 if (gps.locked && gps.nsat >= 4) {
 275                         if ((state != AltosLib.MISSING && state < AltosLib.ao_flight_boost) || gps_pad == null)
 276                                 gps_pad = gps;
 277                         if (gps_pad_altitude == AltosLib.MISSING && gps.alt != AltosLib.MISSING)
 278                                 gps_pad_altitude = gps.alt;
 279                 }
 280                 temp_gps = null;
 281                 prev_gps = gps;
 282         }
 283
 284         /*
 285          * While receiving GPS data, we construct a temporary GPS state
 286          * object and then deliver the result atomically to the listener
 287          */
 288         AltosGPS                temp_gps = null;
 289         int                     temp_gps_sat_tick = AltosLib.MISSING;
 290
 291         public AltosGPS temp_cal_gps() {
 292                 return temp_gps;
 293         }
 294
 295         public void reset_temp_cal_gps() {
 296                 if (temp_gps != null)
 297                         set_cal_gps(temp_gps);
 298         }
 299
 300         public boolean cal_gps_pending() {
 301                 return temp_gps != null;
 302         }
 303
 304         public AltosGPS make_temp_cal_gps(int tick, boolean sats) {
 305                 if (temp_gps == null)
 306                         temp_gps = new AltosGPS(prev_gps);
 307                 if (sats) {
 308                         if (tick != temp_gps_sat_tick)
 309                                 temp_gps.cc_gps_sat = null;
 310                         temp_gps_sat_tick = tick;
 311                 }
 312                 return temp_gps;
 313         }
 314
 315         public int      imu_type = AltosLib.MISSING;
 316
 317         public int      imu_model = AltosLib.MISSING;
 318
 319         public int      mag_model = AltosLib.MISSING;
 320
 321         public void set_imu_type(int imu_type) {
 322                 this.imu_type = imu_type;
 323         }
 324
 325         public void set_imu_model(int imu_model) {
 326                 this.imu_model = imu_model;
 327         }
 328
 329         public void set_mag_model(int mag_model) {
 330                 this.mag_model = mag_model;
 331         }
 332
 333         public double   accel_zero_along, accel_zero_across, accel_zero_through;
 334
 335         public void set_accel_zero(double zero_along, double zero_across, double zero_through) {
 336                 if (zero_along != AltosLib.MISSING) {
 337                         accel_zero_along = zero_along;
 338                         accel_zero_across = zero_across;
 339                         accel_zero_through = zero_through;
 340                 }
 341         }
 342
 343         public double accel_along(double counts) {
 344                 return AltosIMU.convert_accel(counts - accel_zero_along, imu_type, imu_model);
 345         }
 346
 347         public double accel_across(double counts) {
 348                 return AltosIMU.convert_accel(counts - accel_zero_across, imu_type, imu_model);
 349         }
 350
 351         public double accel_through(double counts) {
 352                 return AltosIMU.convert_accel(counts - accel_zero_through, imu_type, imu_model);
 353         }
 354
 355         public double   gyro_zero_roll = AltosLib.MISSING;
 356         public double   gyro_zero_pitch = AltosLib.MISSING;
 357         public double   gyro_zero_yaw = AltosLib.MISSING;
 358
 359         public void set_gyro_zero(double roll, double pitch, double yaw) {
 360                 if (roll != AltosLib.MISSING) {
 361                         gyro_zero_roll = roll;
 362                         gyro_zero_pitch = pitch;
 363                         gyro_zero_yaw = yaw;
 364                         imu_wrap_checked = false;
 365                 }
 366         }
 367
 368         public double gyro_roll(double counts) {
 369                 if (gyro_zero_roll == AltosLib.MISSING || counts == AltosLib.MISSING)
 370                         return AltosLib.MISSING;
 371
 372                 return AltosIMU.gyro_degrees_per_second(counts - gyro_zero_roll, imu_type, imu_model);
 373         }
 374
 375         public double gyro_pitch(double counts) {
 376                 if (gyro_zero_pitch == AltosLib.MISSING || counts == AltosLib.MISSING)
 377                         return AltosLib.MISSING;
 378                 return AltosIMU.gyro_degrees_per_second(counts - gyro_zero_pitch, imu_type, imu_model);
 379         }
 380
 381         public double gyro_yaw(double counts) {
 382                 if (gyro_zero_yaw == AltosLib.MISSING || counts == AltosLib.MISSING)
 383                         return AltosLib.MISSING;
 384                 return AltosIMU.gyro_degrees_per_second(counts - gyro_zero_yaw, imu_type, imu_model);
 385         }
 386
 387         private double gyro_zero_overflow(double first) {
 388                 double v = first / 128.0;
 389                 if (v < 0)
 390                         v = Math.ceil(v);
 391                 else
 392                         v = Math.floor(v);
 393 //              if (v != 0)
 394 //                      System.out.printf("Adjusting gyro axis by %g steps\n", v);
 395                 return v * 128.0;
 396         }
 397
 398         /* Initial TeleMega log format had only 16 bits for gyro cal, so the top 9 bits got lost as the
 399          * cal data are scaled by 512. Use the first sample to adjust the cal value, assuming that it is
 400          * from a time of fairly low rotation speed. Fixed in later TeleMega firmware by storing 32 bits
 401          * of cal values.
 402          */
 403         private boolean imu_wrap_checked = false;
 404
 405         public void check_imu_wrap(double roll, double pitch, double yaw) {
 406                 if (!imu_wrap_checked) {
 407                         gyro_zero_roll += gyro_zero_overflow(roll);
 408                         gyro_zero_pitch += gyro_zero_overflow(pitch);
 409                         gyro_zero_yaw += gyro_zero_overflow(yaw);
 410                         imu_wrap_checked = true;
 411                 }
 412         }
 413
 414         public double mag_along(double along) {
 415                 if (along == AltosLib.MISSING)
 416                         return AltosLib.MISSING;
 417                 return AltosMag.convert_gauss(along, imu_type, mag_model);
 418         }
 419
 420         public double mag_across(double across) {
 421                 if (across == AltosLib.MISSING)
 422                         return AltosLib.MISSING;
 423                 return AltosMag.convert_gauss(across, imu_type, mag_model);
 424         }
 425
 426         public double mag_through(double through) {
 427                 if (through == AltosLib.MISSING)
 428                         return AltosLib.MISSING;
 429                 return AltosMag.convert_gauss(through, imu_type, mag_model);
 430         }
 431
 432         public AltosCalData() {
 433         }
 434
 435         public AltosCalData(AltosConfigData config_data) {
 436                 set_serial(config_data.serial);
 437                 set_ticks_per_sec(100.0);
 438                 set_flight(config_data.flight);
 439                 set_callsign(config_data.callsign);
 440                 set_config(config_data.config_major, config_data.config_minor, config_data.flight_log_max);
 441                 set_firmware_version(config_data.version);
 442                 set_flight_params(config_data.apogee_delay / ticks_per_sec, config_data.apogee_lockout / ticks_per_sec);
 443                 set_pad_orientation(config_data.pad_orientation);
 444                 set_product(config_data.product);
 445                 set_accel_plus_minus(config_data.accel_cal_plus(config_data.pad_orientation), config_data.accel_cal_minus(config_data.pad_orientation));
 446                 set_accel_zero(config_data.accel_zero_along, config_data.accel_zero_across, config_data.accel_zero_through);
 447                 set_ms5607(config_data.ms5607);
 448                 try {
 449                         set_mma655x_inverted(config_data.mma655x_inverted());
 450                 } catch (AltosUnknownProduct up) {
 451                 }
 452                 try {
 453                         set_adxl375_inverted(config_data.adxl375_inverted());
 454                 } catch (AltosUnknownProduct up) {
 455                 }
 456                 try {
 457                         set_adxl375_axis(config_data.adxl375_axis());
 458                 } catch (AltosUnknownProduct up) {
 459                 }
 460                 set_pad_orientation(config_data.pad_orientation);
 461         }
 462 }