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

   1 /*
   2  * Copyright © 2012 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  * You should have received a copy of the GNU General Public License along
  15  * with this program; if not, write to the Free Software Foundation, Inc.,
  16  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  17  */
  18
  19 package org.altusmetrum.altoslib_14;
  20
  21 import java.util.concurrent.*;
  22 import java.io.*;
  23
  24 public class AltosIMU implements Cloneable {
  25         public int              accel_x = AltosLib.MISSING;
  26         public int              accel_y = AltosLib.MISSING;
  27         public int              accel_z = AltosLib.MISSING;
  28
  29         public int              gyro_x = AltosLib.MISSING;
  30         public int              gyro_y = AltosLib.MISSING;
  31         public int              gyro_z = AltosLib.MISSING;
  32
  33         public int              mag_x = AltosLib.MISSING;
  34         public int              mag_y = AltosLib.MISSING;
  35         public int              mag_z = AltosLib.MISSING;
  36
  37         public static final double      counts_per_g_mpu = 2048.0;
  38         public static final double      counts_per_g_bmx = 2048.0;
  39         public static final double      counts_per_g_adxl = 20.5;
  40
  41         private static double counts_per_g(int imu_type) {
  42                 switch (imu_type) {
  43                 case imu_type_telemega_v1_v2:
  44                 case imu_type_telemega_v3:
  45                 case imu_type_easymega_v1:
  46                 case imu_type_easymega_v2:
  47                         return counts_per_g_mpu;
  48                 case  imu_type_telemega_v4:
  49                 case imu_type_easytimer_v1:
  50                         return counts_per_g_bmx;
  51                 case imu_type_easymotor_v2:
  52                         return counts_per_g_adxl;
  53                 default:
  54                         return AltosLib.MISSING;
  55                 }
  56         }
  57
  58         public static double convert_accel(double counts, int imu_type) {
  59                 double cpg = counts_per_g(imu_type);
  60                 if (cpg == AltosLib.MISSING)
  61                         return AltosLib.MISSING;
  62                 return counts / cpg * AltosConvert.gravity;
  63         }
  64
  65         public static final double      GYRO_FULLSCALE_DEGREES_MPU = 2000.0;
  66         public static final double      GYRO_COUNTS_MPU = 32767.0;
  67         public static final double      counts_per_degree_mpu = GYRO_COUNTS_MPU / GYRO_FULLSCALE_DEGREES_MPU;
  68         public static final double      GYRO_FULLSCALE_DEGREES_BMX = 2000.0;
  69         public static final double      GYRO_COUNTS_BMX = 32767.0;
  70         public static final double      counts_per_degree_bmx = GYRO_COUNTS_BMX / GYRO_FULLSCALE_DEGREES_BMX;
  71
  72         private static double counts_per_degree(int imu_type) {
  73                 switch (imu_type) {
  74                 case imu_type_telemega_v1_v2:
  75                 case imu_type_telemega_v3:
  76                 case imu_type_easymega_v1:
  77                 case imu_type_easymega_v2:
  78                         return counts_per_degree_mpu;
  79                 case imu_type_telemega_v4:
  80                 case imu_type_easytimer_v1:
  81                         return counts_per_degree_bmx;
  82                 default:
  83                         return AltosLib.MISSING;
  84                 }
  85         }
  86
  87         public static double gyro_degrees_per_second(double counts, int imu_type) {
  88                 double cpd = counts_per_degree(imu_type);
  89                 if (cpd == AltosLib.MISSING)
  90                         return AltosLib.MISSING;
  91                 return counts / cpd;
  92         }
  93
  94         public static final int imu_axis_x = 0;
  95         public static final int imu_axis_y = 1;
  96         public static final int imu_axis_z = 2;
  97
  98         public static final double MAG_FULLSCALE_GAUSS_MPU = 48.00;     /* 4800µT */
  99         public static final double MAG_COUNTS_MPU = 32767.0;
 100         public static final double counts_per_gauss_mpu = MAG_COUNTS_MPU / MAG_FULLSCALE_GAUSS_MPU;
 101
 102         public static final double counts_per_gauss_bmx = 100.0;        /* BMX driver converts to µT */
 103
 104         public static double counts_per_gauss(int imu_type, int axis) {
 105                 switch(imu_type) {
 106                 case imu_type_telemega_v1_v2:
 107                 case imu_type_easymega_v1:
 108                         return AltosMag.counts_per_gauss;
 109                 case imu_type_telemega_v3:
 110                 case imu_type_easymega_v2:
 111                         return counts_per_gauss_mpu;
 112                 case imu_type_telemega_v4:
 113                 case imu_type_easytimer_v1:
 114                         return 100.0;
 115                 default:
 116                         return AltosLib.MISSING;
 117                 }
 118         }
 119
 120         public static double convert_gauss(double counts, int imu_type, int imu_axis) {
 121                 double cpg = counts_per_gauss(imu_type, imu_axis);
 122                 if (cpg == AltosLib.MISSING)
 123                         return AltosLib.MISSING;
 124                 return counts / cpg;
 125         }
 126
 127         public boolean parse_string(String line) {
 128                 if (!line.startsWith("Accel:"))
 129                         return false;
 130
 131                 String[] items = line.split("\\s+");
 132
 133                 if (items.length >= 8) {
 134                         accel_x = Integer.parseInt(items[1]);
 135                         accel_y = Integer.parseInt(items[2]);
 136                         accel_z = Integer.parseInt(items[3]);
 137                         gyro_x = Integer.parseInt(items[5]);
 138                         gyro_y = Integer.parseInt(items[6]);
 139                         gyro_z = Integer.parseInt(items[7]);
 140                 }
 141                 if (items.length >= 12) {
 142                         mag_x = Integer.parseInt(items[9]);
 143                         mag_y = Integer.parseInt(items[10]);
 144                         mag_z = Integer.parseInt(items[11]);
 145                 }
 146                 return true;
 147         }
 148
 149         public AltosIMU clone() {
 150                 AltosIMU        n = new AltosIMU();
 151
 152                 n.accel_x = accel_x;
 153                 n.accel_y = accel_y;
 154                 n.accel_z = accel_z;
 155
 156                 n.gyro_x = gyro_x;
 157                 n.gyro_y = gyro_y;
 158                 n.gyro_z = gyro_z;
 159
 160                 n.mag_x = mag_x;
 161                 n.mag_y = mag_y;
 162                 n.mag_z = mag_z;
 163
 164                 return n;
 165         }
 166
 167         public static final int imu_type_telemega_v1_v2 = 0;    /* MPU6000 */
 168         public static final int imu_type_telemega_v3 = 1;       /* MPU9250 */
 169         public static final int imu_type_telemega_v4 = 2;       /* BMX160 */
 170
 171         public static final int imu_type_easymega_v1 = 3;       /* MPU6000 */
 172         public static final int imu_type_easymega_v2 = 4;       /* MPU9250 */
 173
 174         public static final int imu_type_easytimer_v1 = 5;      /* BMX160 */
 175
 176         public static final int imu_type_easymotor_v2 = 6;      /* ADXL375 (accel only) */
 177
 178         private int accel_across(int imu_type) {
 179                 switch (imu_type) {
 180                 case imu_type_telemega_v1_v2:
 181                 case imu_type_telemega_v3:
 182                 case imu_type_easymega_v1:
 183                         return accel_x;
 184                 case imu_type_easymega_v2:
 185                         return -accel_y;
 186                 case imu_type_telemega_v4:
 187                 case imu_type_easytimer_v1:
 188                         return -accel_y;
 189                 case imu_type_easymotor_v2:
 190                         return accel_y;
 191                 default:
 192                         return AltosLib.MISSING;
 193                 }
 194         }
 195
 196         private int accel_along(int imu_type) {
 197                 switch (imu_type) {
 198                 case imu_type_telemega_v1_v2:
 199                 case imu_type_telemega_v3:
 200                 case imu_type_easymega_v1:
 201                         return accel_y;
 202                 case imu_type_easymega_v2:
 203                 case imu_type_telemega_v4:
 204                 case imu_type_easytimer_v1:
 205                         return accel_x;
 206                 case imu_type_easymotor_v2:
 207                         return -accel_x;
 208                 default:
 209                         return AltosLib.MISSING;
 210                 }
 211         }
 212
 213         private int accel_through(int imu_type) {
 214                 return accel_z;
 215         }
 216
 217         private int gyro_roll(int imu_type) {
 218                 switch (imu_type) {
 219                 case imu_type_telemega_v1_v2:
 220                 case imu_type_telemega_v3:
 221                 case imu_type_easymega_v1:
 222                         return gyro_y;
 223                 case imu_type_easymega_v2:
 224                 case imu_type_telemega_v4:
 225                 case imu_type_easytimer_v1:
 226                         return gyro_x;
 227                 default:
 228                         return AltosLib.MISSING;
 229                 }
 230         }
 231
 232         private int gyro_pitch(int imu_type) {
 233                 switch (imu_type) {
 234                 case imu_type_telemega_v1_v2:
 235                 case imu_type_telemega_v3:
 236                 case imu_type_easymega_v1:
 237                         return gyro_x;
 238                 case imu_type_easymega_v2:
 239                         return -gyro_y;
 240                 case imu_type_telemega_v4:
 241                 case imu_type_easytimer_v1:
 242                         return -gyro_y;
 243                 default:
 244                         return AltosLib.MISSING;
 245                 }
 246         }
 247
 248         private int gyro_yaw(int imu_type) {
 249                 return gyro_z;
 250         }
 251
 252         public static int mag_across_axis(int imu_type) {
 253                 switch (imu_type) {
 254                 case imu_type_telemega_v1_v2:
 255                 case imu_type_telemega_v3:
 256                 case imu_type_easymega_v1:
 257                         return imu_axis_x;
 258                 case imu_type_easymega_v2:
 259                 case imu_type_telemega_v4:
 260                 case imu_type_easytimer_v1:
 261                         return imu_axis_y;
 262                 default:
 263                         return AltosLib.MISSING;
 264                 }
 265         }
 266
 267         private int mag_across(int imu_type) {
 268                 switch (imu_type) {
 269                 case imu_type_telemega_v1_v2:
 270                 case imu_type_telemega_v3:
 271                 case imu_type_easymega_v1:
 272                         return mag_x;
 273                 case imu_type_easymega_v2:
 274                         return -mag_y;
 275                 case imu_type_telemega_v4:
 276                 case imu_type_easytimer_v1:
 277                         return mag_y;
 278                 default:
 279                         return AltosLib.MISSING;
 280                 }
 281         }
 282
 283         public static int mag_along_axis(int imu_type) {
 284                 switch (imu_type) {
 285                 case imu_type_telemega_v1_v2:
 286                 case imu_type_telemega_v3:
 287                 case imu_type_easymega_v1:
 288                         return imu_axis_y;
 289                 case imu_type_easymega_v2:
 290                 case imu_type_telemega_v4:
 291                 case imu_type_easytimer_v1:
 292                         return imu_axis_x;
 293                 default:
 294                         return AltosLib.MISSING;
 295                 }
 296         }
 297
 298         private int mag_along(int imu_type) {
 299                 switch (imu_type) {
 300                 case imu_type_telemega_v1_v2:
 301                 case imu_type_telemega_v3:
 302                 case imu_type_easymega_v1:
 303                         return mag_y;
 304                 case imu_type_easymega_v2:
 305                 case imu_type_telemega_v4:
 306                 case imu_type_easytimer_v1:
 307                         return mag_x;
 308                 default:
 309                         return AltosLib.MISSING;
 310                 }
 311         }
 312
 313         private static boolean is_primary_accel(int imu_type) {
 314                 switch (imu_type) {
 315                 case imu_type_easytimer_v1:
 316                         return true;
 317                 default:
 318                         return false;
 319                 }
 320         }
 321
 322         public static int mag_through_axis(int imu_type) {
 323                 return imu_axis_z;
 324         }
 325
 326         private int mag_through(int imu_type) {
 327                 return mag_z;
 328         }
 329
 330         static public void provide_data(AltosDataListener listener, AltosLink link, int imu_type) throws InterruptedException {
 331                 try {
 332                         AltosIMU        imu = new AltosIMU(link);
 333                         AltosCalData    cal_data = listener.cal_data();
 334
 335                         cal_data.set_imu_type(imu_type);
 336
 337                         if (imu != null) {
 338                                 if (imu.gyro_x != AltosLib.MISSING) {
 339                                         cal_data.set_gyro_zero(0, 0, 0);
 340                                         listener.set_gyro(cal_data.gyro_roll(imu.gyro_roll(imu_type)),
 341                                                           cal_data.gyro_pitch(imu.gyro_pitch(imu_type)),
 342                                                           cal_data.gyro_yaw(imu.gyro_yaw(imu_type)));
 343                                 }
 344                                 listener.set_accel_ground(cal_data.accel_along(imu.accel_along(imu_type)),
 345                                                           cal_data.accel_across(imu.accel_across(imu_type)),
 346                                                           cal_data.accel_through(imu.accel_through(imu_type)));
 347                                 listener.set_accel(cal_data.accel_along(imu.accel_along(imu_type)),
 348                                                    cal_data.accel_across(imu.accel_across(imu_type)),
 349                                                    cal_data.accel_through(imu.accel_through(imu_type)));
 350                                 if (is_primary_accel(imu_type)) {
 351                                         int accel = imu.accel_along(imu_type);
 352                                         if (!cal_data.adxl375_inverted)
 353                                                 accel = -accel;
 354                                         if (cal_data.pad_orientation == 1)
 355                                                 accel = -accel;
 356                                         listener.set_acceleration(cal_data.acceleration(accel));
 357                                 }
 358                                 if (imu.mag_x != AltosLib.MISSING) {
 359                                         listener.set_mag(cal_data.mag_along(imu.mag_along(imu_type)),
 360                                                          cal_data.mag_across(imu.mag_across(imu_type)),
 361                                                          cal_data.mag_through(imu.mag_through(imu_type)));
 362                                 }
 363                         }
 364                 } catch (TimeoutException te) {
 365                 }
 366         }
 367
 368         public AltosIMU() {
 369                 accel_x = AltosLib.MISSING;
 370                 accel_y = AltosLib.MISSING;
 371                 accel_z = AltosLib.MISSING;
 372
 373                 gyro_x = AltosLib.MISSING;
 374                 gyro_y = AltosLib.MISSING;
 375                 gyro_z = AltosLib.MISSING;
 376
 377                 mag_x = AltosLib.MISSING;
 378                 mag_y = AltosLib.MISSING;
 379                 mag_z = AltosLib.MISSING;
 380         }
 381
 382         public AltosIMU(AltosLink link) throws InterruptedException, TimeoutException {
 383                 this();
 384                 link.printf("I\n");
 385                 for (;;) {
 386                         String line = link.get_reply_no_dialog(5000);
 387                         if (line == null) {
 388                                 throw new TimeoutException();
 389                         }
 390                         if (parse_string(line))
 391                                 break;
 392                 }
 393         }
 394 }