src/hacks/calibrationtest/calibrationtest.c

   1 /* Copyright (C) 2008 Sean D'Epagnier <sean@depagnier.com>
   2  *
   3  * This Program is free software; you can redistribute it and/or
   4  * modify it under the terms of the GNU General Public
   5  * License as published by the Free Software Foundation; either
   6  * version 3 of the License, or (at your option) any later version.
   7  *
   8  * This program is distributed in the hope that it will be useful,
   9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  * GNU General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License
  14  * along with this program; if not, write to the Free Software
  15  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  16  *
  17  * For more information on the GPL, please go to:
  18  * http://www.gnu.org/copyleft/gpl.html
  19  */
  20
  21 #include <stdint.h>
  22 #include <math.h>
  23
  24 #include <calibration/calibrationserver.h>
  25 #include <calibration/calc.h>
  26
  27 #include <linalg/vector.h>
  28
  29 #define AZMODE
  30
  31 int main(int argc, char *argv[])
  32 {
  33 #if 1
  34   accelX[0] = -149.1;
  35   accelX[1] = -4.6;
  36   accelX[2] = 123.0;
  37   accelX[3] = 1328.9;
  38
  39   magX[0] = 1.0 / 50100;
  40   magX[1] =  .992;
  41   magX[2] = 1.05;
  42   magX[3] = -0.106;
  43   magX[4] =  0.00694;
  44   magX[5] = -0.0736;
  45   magX[6] = -7040;
  46   magX[7] = -9120;
  47   magX[8] = -15360;
  48
  49 #if 0 /* l d */
  50   magX[6] = -6585;
  51   magX[7] = -9120;
  52   magX[8] = -15430;
  53 #endif
  54 #if 0 /* u r */
  55   magX[6] = -7000;
  56   magX[7] = -9820;
  57   magX[8] = -15150;
  58 #endif
  59 #if 0 /* s (d and r) */
  60   magX[6] = -6010;
  61   magX[7] = -8750;
  62   magX[8] = -15150;
  63 #endif
  64
  65   magalignX[0] = 0.002;
  66   magalignX[1] = -.003;
  67   magalignX[2] = 0.021;
  68
  69 #else
  70   accelX[0] = 198.3;
  71   accelX[1] = 60;
  72   accelX[2] = -42.5;
  73   accelX[3] = 1327.1;
  74
  75   magX[0] = 1.0 / 50400;
  76   magX[1] =  .974;
  77   magX[2] = 1.05;
  78   magX[3] = -0.11;
  79   magX[4] =  0.0558;
  80   magX[5] = -0.0679;
  81
  82   magX[6] = 38500;
  83   magX[7] = 19600;
  84   magX[8] = -29800;
  85
  86 #if 0 /* r */
  87   magX[6] = 37200;
  88   magX[7] = 18300;
  89   magX[8] = -29700;
  90 #endif
  91 #if 0 /* u */
  92   magX[6] = 37540;
  93   magX[7] = 20360;
  94   magX[8] = -29550;
  95 #endif
  96 #if 0 /* l */
  97   magX[6] = 37580;
  98   magX[7] = 20250;
  99   magX[8] = -30070;
 100 #endif
 101 #if 1 /* d */
 102   magX[6] = 37410;
 103   magX[7] = 21060;
 104   magX[8] = -30520;
 105 #endif
 106
 107   magalignX[0] = 0.0020;
 108   magalignX[1] = 0.000453;
 109   magalignX[2] = 0.0384;
 110
 111 #endif
 112
 113   accellaseralignment[1] = 0.00;
 114   accellaseralignment[2] = -0.013;
 115   accellaseralignment[3] = -.01;
 116
 117   accellaseralignment[0] = sqrt(1 - magnitude2(accellaseralignment+1));
 118   memcpy(maglaseralignment, accellaseralignment, sizeof accellaseralignment);
 119
 120   float corrazimuth = 0, total=0, totalm=0;
 121   char buffer[1024];
 122   static float asave[72];
 123   int i=0;
 124   while(fgets(buffer, sizeof buffer, stdin)) {
 125     if(sscanf(buffer, "rawaccel: %ld %ld %ld\n",
 126               accel.filtered + 0,
 127               accel.filtered + 1,
 128               accel.filtered + 2) == 3) {
 129       int i;
 130       for(i = 0; i<3; i++)
 131         accel.filtered[i] <<= 4;
 132     }
 133
 134     if(sscanf(buffer, "rawmag: %ld %ld %ld\n",
 135               mag.filtered + 0,
 136               mag.filtered + 1,
 137               mag.filtered + 2) == 3) {
 138
 139       calc_laser_incline_azimuth();
 140
 141       float a[3];
 142       calc_get_calibrated_accel(a);
 143
 144       float diff = azimuth - corrazimuth;
 145       if(diff < -50)
 146         diff += 360;
 147       if(diff > 50)
 148         diff -= 360;
 149
 150       float m = 1-calibration_get_mag_magnitude();
 151       totalm += m*m;
 152
 153       total += diff;
 154       asave[i++] = diff;
 155
 156 #ifdef AZMODE
 157       corrazimuth += 5;
 158 #endif
 159     }
 160   }
 161
 162   float dev = 0;
 163   corrazimuth = 0;
 164   int j = 0;
 165   for(j=0; j<i; j++) {
 166     float d = asave[j] - total / i;
 167
 168     printf("%.2f %.2f %.2f\n", corrazimuth, d, asave[j]);
 169
 170 #ifdef AZMODE
 171       corrazimuth += 5;
 172 #else
 173       corrazimuth += 1;
 174 #endif
 175
 176     dev += d*d;
 177   }
 178
 179   fprintf(stderr, "mag dev: %.5f\n", sqrt(totalm / 72));
 180   fprintf(stderr, "dev: %.5f\n", sqrt(dev / 72));
 181
 182   return 0;
 183 }