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_11;
  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         }
  71
  72         public int              config_major = AltosLib.MISSING;
  73         public int              config_minor = AltosLib.MISSING;
  74         public int              flight_log_max = AltosLib.MISSING;
  75
  76         public void set_config(int major, int minor, int log_max) {
  77                 if (major != AltosLib.MISSING)
  78                         config_major = major;
  79                 if (minor != AltosLib.MISSING)
  80                         config_minor = minor;
  81                 if (log_max != AltosLib.MISSING)
  82                         flight_log_max = log_max;
  83         }
  84
  85         public double           apogee_delay = AltosLib.MISSING;
  86         public double           main_deploy = AltosLib.MISSING;
  87
  88         public void set_flight_params(double apogee_delay, double main_deploy) {
  89                 if (apogee_delay != AltosLib.MISSING)
  90                         this.apogee_delay = apogee_delay;
  91                 if (main_deploy != AltosLib.MISSING)
  92                         this.main_deploy = main_deploy;
  93         }
  94
  95         public double           accel_plus_g = AltosLib.MISSING;
  96         public double           accel_minus_g = AltosLib.MISSING;
  97         public double           ground_accel = AltosLib.MISSING;
  98
  99         public void set_accel_plus_minus(double plus, double minus) {
 100                 if (plus != AltosLib.MISSING && minus != AltosLib.MISSING) {
 101                         accel_plus_g = plus;
 102                         accel_minus_g = minus;
 103                 }
 104         }
 105
 106         public void set_ground_accel(double ground_accel) {
 107                 if (ground_accel != AltosLib.MISSING)
 108                         this.ground_accel = ground_accel;
 109         }
 110
 111         /* Raw acceleration value */
 112         public double           accel = AltosLib.MISSING;
 113
 114         public void set_accel(double accel) {
 115                 this.accel = accel;
 116         }
 117
 118         public boolean mma655x_inverted = false;
 119
 120         public void set_mma655x_inverted(boolean inverted) {
 121                 mma655x_inverted = inverted;
 122         }
 123
 124         public int pad_orientation = AltosLib.MISSING;
 125
 126         public void set_pad_orientation(int orientation) {
 127                 if (orientation != AltosLib.MISSING)
 128                         pad_orientation = orientation;
 129         }
 130
 131         /* Compute acceleration */
 132         public double acceleration(double sensor) {
 133                 return AltosConvert.acceleration_from_sensor(sensor, accel_plus_g, accel_minus_g, ground_accel);
 134         }
 135
 136         public AltosMs5607      ms5607 = null;
 137
 138         public void set_ms5607(AltosMs5607 ms5607) {
 139                 this.ms5607 = ms5607;
 140         }
 141
 142         public double           ground_pressure = AltosLib.MISSING;
 143         public double           ground_altitude = AltosLib.MISSING;
 144
 145         public void set_ground_pressure(double ground_pressure) {
 146                 if (ground_pressure != AltosLib.MISSING) {
 147                         this.ground_pressure = ground_pressure;
 148                         this.ground_altitude = AltosConvert.pressure_to_altitude(ground_pressure);
 149                 }
 150         }
 151
 152         public void set_ground_altitude(double ground_altitude) {
 153                 if (ground_altitude != AltosLib.MISSING)
 154                         this.ground_altitude = ground_altitude;
 155         }
 156
 157         /* Compute pressure */
 158
 159         public AltosPresTemp pressure_ms5607(int raw_pres, int raw_temp) {
 160                 if (ms5607 == null)
 161                         return new AltosPresTemp(AltosLib.MISSING, AltosLib.MISSING);
 162                 return ms5607.pres_temp(raw_pres, raw_temp);
 163         }
 164
 165         public int              tick = AltosLib.MISSING;
 166         private int             prev_tick = AltosLib.MISSING;
 167
 168         public void set_tick(int tick) {
 169                 if (tick != AltosLib.MISSING) {
 170                         if (prev_tick != AltosLib.MISSING) {
 171                                 while (tick < prev_tick - 1000) {
 172                                         tick += 65536;
 173                                 }
 174                         }
 175                         this.tick = tick;
 176                 }
 177         }
 178
 179         public int              boost_tick = AltosLib.MISSING;
 180
 181         public void set_boost_tick() {
 182                 boost_tick = tick;
 183         }
 184
 185         public double time() {
 186                 if (tick == AltosLib.MISSING)
 187                         return AltosLib.MISSING;
 188                 if (boost_tick == AltosLib.MISSING)
 189                         return AltosLib.MISSING;
 190                 return (tick - boost_tick) / 100.0;
 191         }
 192
 193         public double boost_time() {
 194                 if (boost_tick == AltosLib.MISSING)
 195                         return AltosLib.MISSING;
 196                 return boost_tick / 100.0;
 197         }
 198
 199         public int              state = AltosLib.MISSING;
 200
 201         public void set_state(int state) {
 202                 if (state >= AltosLib.ao_flight_boost && boost_tick == AltosLib.MISSING)
 203                         set_boost_tick();
 204                 this.state = state;
 205         }
 206
 207         public AltosGPS         gps_pad = null;
 208
 209         public double           gps_pad_altitude = AltosLib.MISSING;
 210
 211         public void set_gps(AltosGPS gps) {
 212                 if ((state != AltosLib.MISSING && state < AltosLib.ao_flight_boost) || gps_pad == null)
 213                         gps_pad = gps;
 214                 if (gps_pad_altitude == AltosLib.MISSING && gps.alt != AltosLib.MISSING)
 215                         gps_pad_altitude = gps.alt;
 216         }
 217
 218         /*
 219          * While receiving GPS data, we construct a temporary GPS state
 220          * object and then deliver the result atomically to the listener
 221          */
 222         AltosGPS                temp_gps = null;
 223         int                     temp_gps_sat_tick = AltosLib.MISSING;
 224
 225         public AltosGPS temp_gps() {
 226                 return temp_gps;
 227         }
 228
 229         public void reset_temp_gps() {
 230                 if (temp_gps != null) {
 231                         if (temp_gps.locked && temp_gps.nsat >= 4)
 232                                 set_gps(temp_gps);
 233                         temp_gps = null;
 234                 }
 235         }
 236
 237         public boolean gps_pending() {
 238                 return temp_gps != null;
 239         }
 240
 241         public AltosGPS make_temp_gps(int tick, boolean sats) {
 242                 if (temp_gps == null)
 243                         temp_gps = new AltosGPS();
 244                 if (sats) {
 245                         if (tick != temp_gps_sat_tick)
 246                                 temp_gps.cc_gps_sat = null;
 247                         temp_gps_sat_tick = tick;
 248                 }
 249                 return temp_gps;
 250         }
 251
 252         public double   accel_zero_along, accel_zero_across, accel_zero_through;
 253
 254         public void set_accel_zero(double zero_along, double zero_across, double zero_through) {
 255                 if (zero_along != AltosLib.MISSING) {
 256                         accel_zero_along = zero_along;
 257                         accel_zero_across = zero_across;
 258                         accel_zero_through = zero_through;
 259                 }
 260         }
 261
 262         public double accel_along(double counts) {
 263                 return AltosIMU.convert_accel(counts - accel_zero_along);
 264         }
 265
 266         public double accel_across(double counts) {
 267                 return AltosIMU.convert_accel(counts - accel_zero_across);
 268         }
 269
 270         public double accel_through(double counts) {
 271                 return AltosIMU.convert_accel(counts - accel_zero_through);
 272         }
 273
 274         public double   gyro_zero_roll, gyro_zero_pitch, gyro_zero_yaw;
 275
 276         public void set_gyro_zero(double roll, double pitch, double yaw) {
 277                 if (roll != AltosLib.MISSING) {
 278                         gyro_zero_roll = roll;
 279                         gyro_zero_pitch = pitch;
 280                         gyro_zero_yaw = yaw;
 281                 }
 282         }
 283
 284         public double gyro_roll(double counts) {
 285                 if (gyro_zero_roll == AltosLib.MISSING || counts == AltosLib.MISSING)
 286                         return AltosLib.MISSING;
 287                 return AltosIMU.convert_gyro(counts - gyro_zero_roll);
 288         }
 289
 290         public double gyro_pitch(double counts) {
 291                 if (gyro_zero_pitch == AltosLib.MISSING || counts == AltosLib.MISSING)
 292                         return AltosLib.MISSING;
 293                 return AltosIMU.convert_gyro(counts - gyro_zero_pitch);
 294         }
 295
 296         public double gyro_yaw(double counts) {
 297                 if (gyro_zero_yaw == AltosLib.MISSING || counts == AltosLib.MISSING)
 298                         return AltosLib.MISSING;
 299                 return AltosIMU.convert_gyro(counts - gyro_zero_yaw);
 300         }
 301
 302         private double gyro_zero_overflow(double first) {
 303                 double v = first / 128.0;
 304                 if (v < 0)
 305                         v = Math.ceil(v);
 306                 else
 307                         v = Math.floor(v);
 308                 return v * 128.0;
 309         }
 310
 311         public void check_imu_wrap(double roll, double pitch, double yaw) {
 312                 gyro_zero_roll += gyro_zero_overflow(roll);
 313                 gyro_zero_pitch += gyro_zero_overflow(pitch);
 314                 gyro_zero_yaw += gyro_zero_overflow(yaw);
 315         }
 316
 317         public double mag_along(double along) {
 318                 if (along == AltosLib.MISSING)
 319                         return AltosLib.MISSING;
 320                 return AltosMag.convert_gauss(along);
 321         }
 322
 323         public double mag_across(double across) {
 324                 if (across == AltosLib.MISSING)
 325                         return AltosLib.MISSING;
 326                 return AltosMag.convert_gauss(across);
 327         }
 328
 329         public double mag_through(double through) {
 330                 if (through == AltosLib.MISSING)
 331                         return AltosLib.MISSING;
 332                 return AltosMag.convert_gauss(through);
 333         }
 334
 335         public AltosCalData() {
 336         }
 337
 338         public AltosCalData(AltosConfigData config_data) {
 339                 set_serial(config_data.serial);
 340                 set_flight(config_data.flight);
 341                 set_callsign(config_data.callsign);
 342                 set_accel_plus_minus(config_data.accel_cal_plus, config_data.accel_cal_minus);
 343                 set_ms5607(config_data.ms5607);
 344                 try {
 345                         set_mma655x_inverted(config_data.mma655x_inverted());
 346                 } catch (AltosUnknownProduct up) {
 347                 }
 348                 set_pad_orientation(config_data.pad_orientation);
 349         }
 350 }