search

kriswiner / MPU9250

Arduino sketches for MPU9250 9DoF with AHRS sensor fusion
1.04k stars 471 forks source link

Port for PIC24 and problems #96

Open TopperBG opened 7 years ago

TopperBG commented 7 years ago

First thanks for hard work.

I've try to port whole library to PIC24 and face some problems. Based on https://github.com/kriswiner/MPU-9250/blob/dea8453be4abd6c94612a13a2ddb5db184fed7cb/MPU9250BasicAHRS_t3.ino Board is that https://www.seeedstudio.com/Grove-IMU-9DOF-v2.0-p-2400.html Sensors response to Who_I_Am propperly. MPU init pass fine. Startup procedure is:

void Setup_MPU9250(){
   fprintf(PC,"\n   *** Setup MPU 9250 unit ***\n\r");       
    if (MPU9250WIA()) {                      
         fprintf(PC,"\n Reset MPU9250");         
         resetMPU9250();
         fprintf(PC,"\n Calibration MPU start");
         calibrateMPU9250(gyroBias, accelBias);  // Calibrate gyro and accelerometers, load biases in bias registers
         fprintf(PC,"\n accel biases (mg) %f %f %f",1000.*accelBias[0],1000.*accelBias[1],1000.*accelBias[2]);
         fprintf(PC,"\n gyro biases (mg) %f %f %f",gyroBias[0],gyroBias[1],gyroBias[2]);  

         fprintf(PC,"\n Init MPU9250");
        init_MPU9250(); 
   }                                         
   if (CompassWIA()) {                               
         fprintf(PC,"\n Init AK8963");      
         InitAK8963(magCalibration);        
         fprintf(PC,"\n Calibrate AK8963");      
         MagCalMPU9250(magBias,magScale);
         fprintf(PC,"\n AK8963 mag biases (mG) %f %f %f",magBias[0],magBias[1],magBias[2]);
         fprintf(PC,"\n AK8963 mag scale (mG) %f %f %f",magScale[0],magScale[1],magScale[2]);  
   }                                                             
   fprintf(PC,"\n   *** End MPU 9250 setup ***\n\r");
}

1.First problem is calibration function according the code. After enable and quick disable FIFO (even within 1ms) the FIFO Counter FIFO_COUNTH is 65535 and samples to average are 5461. Doesn't change with manipulate time interval between:

i2c_WriteByte_MPU9250(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO
i2c_WriteByte_MPU9250(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150)

and

// At end of sample accumulation, turn off FIFO sensor read i2c_WriteByte_MPU9250(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO i2c_ReadByteS_MPU9250(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count

So I've put hardcoded packet_counter =40; See results below

2.Second problems is ReadMagData. When started CalibrateMag and must be read 128 samples from Mag, first was read after that ST1 stays 0. Mode for Mag is single mode (0001)

Here is output with heavy debug:

Setup MPU 9250 unit I AM 0x71 MPU9250 is online...

Reset MPU9250 Init MPU9250 Calibration MPU start Calibration MPU ... Calibration MPU 5461 samples for avrg. FIFO counter is 65535d

0 #1 #2 #3 #4 #5 #6 #7 #8 #9 #10 #11 #12 #13 #14 #15 #16 #17 #18 #19 #20 #21 #22 >#23 #24 #25 #26 #27 #28 #29 #30 #31 #32 #33 #34 #35 #36 #37 #38 #39

accel biases (mg) 0.00 0.00 1000.00 gyro biases (mg) 0.00 0.00 0.00

I AM 0x48 Compass is online...

Init AK8963 ASAX raw data read X-Axis sensitivity adjustment value 1.16 Y-Axis sensitivity adjustment value 1.16 Z-Axis sensitivity adjustment value 1.12 magCalibration 1066696704 1066729472 1066369024

End MPU 9250 setup

MainLoop <<<

ReadMagData AK8963_ST1 ST1=1... Get 6 raw mag data MagRaw data: mx 0 my 0 mz 0 X-acceleration: 9.27 mg Y-acceleration: 14.40 mg Z-acceleration: 1030.27 mg X-gyro rate: 71.64 degrees/sec Y-gyro rate: 82.92 degrees/sec Z-gyro rate: 91.39 degrees/sec X-mag field: -463 mG Y-mag field: -120 mG Z-mag field: -118 mG Temperature is 27.70 degrees C q0 = 1.00 qx = 0.00 qy = 0.00 qz = 0.00

Yaw -13.80, Pitch 0.00, Roll 0.00:

ReadMagData AK8963_ST1 ST1=0...

ReadMagData AK8963_ST1 ST1=0...

ReadMagData AK8963_ST1 ST1=0...

ReadMagData AK8963_ST1 ST1=0...

Any ideas ?

kriswiner commented 7 years ago

Seems like you are able to read the sensor data. What's the problem?

TopperBG commented 7 years ago

Calibration can't be done (FIFO isn't work?). SelfTest too. Yaw, pitch and Roll didn't calculate nor even wrong. And at last can read only one packet of samples from Mag, after that ST1 reg stays 0 - no DRDY

kriswiner commented 7 years ago

sounds like I2C is blocked.

TopperBG commented 7 years ago

Hm, strange. Every command through I2C have at least 10ms delay. On MPU mode change - 100ms.... That numbers are strange for 10ms delay and don't change at other delay: Calibration MPU 5461 samples for avrg. FIFO counter is 65535d

kriswiner commented 7 years ago

10 ms delay is forever. There is something wrong with your setup. What pullups are you using on the I2C bus?

TopperBG commented 7 years ago

10K are pullup resistors on pins. According delays I follow your code as:

 i2c_WriteByte_MPU9250(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);  // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
        delay_ms(100); // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt

Or when change power mode of MPU.

kriswiner commented 7 years ago

This is just for the initialization, not for taking data.

TopperBG commented 7 years ago

Ok, will remove any delays in W/R byte(s) to I2C and will report back.

TopperBG commented 7 years ago

Sorry for delay was at bussines trip. So, removed all delays from I2C r/w functions. Put here outputs with additional debug fro SelfTest and Calibration procedures:

Setup MPU 9250 unit

I AM 0x71 MPU9250 is online...

SelfTest MPU9250 Results: x-axis self test: acceleration trim within : 10.30 % of factory value y-axis self test: acceleration trim within : -101.26 % of factory value z-axis self test: acceleration trim within : 35.36 % of factory value x-axis self test: gyration trim within : -103.48 % of factory value y-axis self test: gyration trim within : -99.01 % of factory value z-axis self test: gyration trim within : 59.82 % of factory value

Calibration MPU start

FIFO_CountH=0, FIFO_CountL=0

0 samples for avrg. FIFO counter is 0

accel_bias[0] 6306 accel_bias[1] 6306 accel_bias[2] 6306 gyro_bias[0] 6306 gyro_bias[1] 6306 gyro_bias[2] 6306 accel biases (mg) 384.88 384.88 -615.11 mg gyro biases (mg) 48.13 48.13 48.13 o/s Init MPU9250

I assume that FIFO is not working or is not correct setup? Here is the source of Calibration:

void calibrateMPU9250 (float * dest1, float * dest2) {
      unsigned int8 data[12]; // data array to hold accelerometer and gyro x, y, z, data
      unsigned int16 ii, packet_count=0, fifo_count=0;
      int32 gyro_bias[3] = { 0, 0, 0 }, accel_bias[3] = { 0, 0, 0 };

      // reset device, reset all registers, clear gyro and accelerometer bias registers
      i2c_WriteByte_MPU9250(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
      delay_ms(100);                                                        

      // get stable time source
      // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
      i2c_WriteByte_MPU9250(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);   
      i2c_WriteByte_MPU9250(MPU9250_ADDRESS, PWR_MGMT_2, 0x00);
      delay_ms(200);                                                     

      // Configure device for bias calculation
      i2c_WriteByte_MPU9250(MPU9250_ADDRESS, INT_ENABLE, 0x00);   // Disable all interrupts
      i2c_WriteByte_MPU9250(MPU9250_ADDRESS, FIFO_EN, 0x00);      // Disable FIFO
      i2c_WriteByte_MPU9250(MPU9250_ADDRESS, PWR_MGMT_1, 0x00);   // Turn on internal clock source 
      i2c_WriteByte_MPU9250(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
      i2c_WriteByte_MPU9250(MPU9250_ADDRESS, USER_CTRL, 0x00);    // Disable FIFO and I2C master modes
      i2c_WriteByte_MPU9250(MPU9250_ADDRESS, USER_CTRL, 0x0C);    // Reset FIFO and DMP (0x0C)
      delay_ms(15);                                                                        

      // Configure MPU9250 gyro and accelerometer for bias calculation     
      i2c_WriteByte_MPU9250(MPU9250_ADDRESS, CONFIG, BANDWITH_GYRO_184_HZ);      // Set low-pass filter to 188 Hz
      i2c_WriteByte_MPU9250(MPU9250_ADDRESS, SMPLRT_DIV, SAMPLE_RATE_DIVIDER_0);  // Set sample rate to 1 kHz
      i2c_WriteByte_MPU9250(MPU9250_ADDRESS, GYRO_CONFIG, GYRO_FULL_SCALE_250_DPS);  // Set gyro full-scale to 250 degrees per second, maximum sensitivity
      i2c_WriteByte_MPU9250(MPU9250_ADDRESS, ACCEL_CONFIG, ACC_FULL_SCALE_2_G); // Set accelerometer full-scale to 2 g, maximum sensitivity

      unsigned int16  gyrosensitivity  = 131;   // = 131 LSB/degrees/sec         
      unsigned int16  accelsensitivity = 16384;  // = 16384 LSB/g         

    // Configure FIFO to capture accelerometer and gyro data for bias calculation
     i2c_WriteByte_MPU9250(MPU9250_ADDRESS, USER_CTRL, 0x40);   // Enable FIFO  
     i2c_WriteByte_MPU9250(MPU9250_ADDRESS, FIFO_EN, 0x78);     // Enable gyro and accelerometer sensors for FIFO  (max size 512 bytes in MPU-9150)

     delay_ms(100); // accumulate 40 samples in 40 milliseconds = 480 bytes

   // At end of sample accumulation, turn off FIFO sensor read
     i2c_WriteByte_MPU9250(MPU9250_ADDRESS, FIFO_EN, 0x00);        // Disable gyro and accelerometer sensors for FIFO
     i2c_ReadByteS_MPU9250(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count
     delay_ms(20);      
     fprintf(PC,"\n\n FIFO_CountH=%u, FIFO_CountL=%u", data[0],data[1]);  
     fifo_count = (unsigned int16)(data[0] << 8) | data[1]; 
     packet_count = (fifo_count/12);  // How many sets of full gyro and accelerometer data for averaging 
          fprintf(PC,"\n >>> %d samples for avrg. FIFO counter is %u \n",packet_count,fifo_count);
//!          packet_count = 10;                              

     for (ii = 0; ii < packet_count; ii++) {        
          int16 accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};     
          i2c_ReadByteS_MPU9250(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
          accel_temp[0] = (int16) (((int16)data[0] << 8) | data[1]  ) ;  // Form signed 16-bit integer for each sample in FIFO 
          accel_temp[1] = (int16) (((int16)data[2] << 8) | data[3]  ) ;
          accel_temp[2] = (int16) (((int16)data[4] << 8) | data[5]  ) ;             
          gyro_temp[0]  = (int16) (((int16)data[6] << 8) | data[7]  ) ;    
          gyro_temp[1]  = (int16) (((int16)data[8] << 8) | data[9]  ) ;
          gyro_temp[2]  = (int16) (((int16)data[10] << 8) | data[11]) ;         

          accel_bias[0] += (int32) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
          accel_bias[1] += (int32) accel_temp[1];   
          accel_bias[2] += (int32) accel_temp[2];
          gyro_bias[0]  += (int32) gyro_temp[0];   
          gyro_bias[1]  += (int32) gyro_temp[1];                                             
          gyro_bias[2]  += (int32) gyro_temp[2]; 
          fprintf(PC,"\n Cicle %d:",ii);   
          fprintf(PC,"\n RawData %d %d %d %d %d %d %d %d %d %d %d %d",data[0],data[1],data[2],data[3],data[4],data[5],data[6],data[7],data[8],data[9],data[10],data[11]);
          fprintf(PC,"\n accel_temp %d %d %d",accel_temp[0],accel_temp[1],accel_temp[2]);
          fprintf(PC,"\n gyro_temp %d %d %d",gyro_temp[0],gyro_temp[1],gyro_temp[2]);  
          fprintf(PC,"\n accel_bias %d %d %d",accel_bias[0],accel_bias[1],accel_bias[2]);
          fprintf(PC,"\n gyro_temp %d %d %d",gyro_bias[0],gyro_bias[1],gyro_bias[2]);  
   }                                                       
    accel_bias[0] /= (int32) packet_count; // Normalize sums to get average count biases
    accel_bias[1] /= (int32) packet_count;        
    accel_bias[2] /= (int32) packet_count;                 
    gyro_bias[0]  /= (int32) packet_count;        
    gyro_bias[1]  /= (int32) packet_count;
    gyro_bias[2]  /= (int32) packet_count; 

fprintf(PC,"\n accel_bias[0] %d accel_bias[1] %d accel_bias[2] %d",accel_bias[0],accel_bias[1],accel_bias[2]);
fprintf(PC,"\n gyro_bias[0] %d gyro_bias[1] %d gyro_bias[2] %d",accel_bias[0],accel_bias[1],accel_bias[2]);

  if(accel_bias[2] > 0L) {accel_bias[2] -= (int32) accelsensitivity;}  // Remove gravity from the z-axis accelerometer bias calculation
  else {accel_bias[2] += (int32) accelsensitivity;}  
  // Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
      data[0] = (-gyro_bias[0] / 4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
      data[1] = (-gyro_bias[0] / 4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
      data[2] = (-gyro_bias[1] / 4 >> 8) & 0xFF;         
      data[3] = (-gyro_bias[1] / 4) & 0xFF;
      data[4] = (-gyro_bias[2] / 4 >> 8) & 0xFF;
      data[5] = (-gyro_bias[2] / 4) & 0xFF;

      /// Push gyro biases to hardware registers        
      i2c_WriteByte_MPU9250(MPU9250_ADDRESS, XG_OFFSET_H, data[0]);
      i2c_WriteByte_MPU9250(MPU9250_ADDRESS, XG_OFFSET_L, data[1]);
      i2c_WriteByte_MPU9250(MPU9250_ADDRESS, YG_OFFSET_H, data[2]);
      i2c_WriteByte_MPU9250(MPU9250_ADDRESS, YG_OFFSET_L, data[3]);                   
      i2c_WriteByte_MPU9250(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]);
      i2c_WriteByte_MPU9250(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]);     

      dest1[0] = (float)gyro_bias[0] / (float)gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction
      dest1[1] = (float)gyro_bias[1] / (float)gyrosensitivity;
      dest1[2] = (float)gyro_bias[2] / (float)gyrosensitivity;

      // Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
      // factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
      // non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
      // compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
      // the accelerometer biases calculated above must be divided by 8.

      int32 accel_bias_reg[3] = { 0, 0, 0 }; // A place to hold the factory accelerometer trim biases
      i2c_ReadByteS_MPU9250(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
      accel_bias_reg[0] = (int16)((int16)data[0] << 8) | data[1];
      i2c_ReadByteS_MPU9250(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);
      accel_bias_reg[1] = (int16)((int16)data[0] << 8) | data[1];
      i2c_ReadByteS_MPU9250(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
      accel_bias_reg[2] = (int16)((int16)data[0] << 8) | data[1];    

      unsigned int32 mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
      unsigned int8 mask_bit[3] = { 0, 0, 0 }; // Define array to hold mask bit for each accelerometer bias axis

      for (ii = 0; ii < 3; ii++) {
         if (accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit
      }                                 
      // Construct total accelerometer bias, including calculated average accelerometer bias from above
      accel_bias_reg[0] -= (accel_bias[0] / 8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
      accel_bias_reg[1] -= (accel_bias[1] / 8);
      accel_bias_reg[2] -= (accel_bias[2] / 8);

      data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
      data[1] = (accel_bias_reg[0]) & 0xFF;
      data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers
      data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
      data[3] = (accel_bias_reg[1]) & 0xFF;
      data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers
      data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
      data[5] = (accel_bias_reg[2]) & 0xFF;
      data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers

      // Apparently this is not working for the acceleration biases in the MPU-9250
      // Are we handling the temperature correction bit properly?
      // Push accelerometer biases to hardware registers
     /* i2c_WriteByte_MPU9250 (MPU9250_ADDRESS, XA_OFFSET_H, data[0]);
      i2c_WriteByte_MPU9250 (MPU9250_ADDRESS, XA_OFFSET_L, data[1]);
      i2c_WriteByte_MPU9250 (MPU9250_ADDRESS, YA_OFFSET_H, data[2]);             
      i2c_WriteByte_MPU9250 (MPU9250_ADDRESS, YA_OFFSET_L, data[3]);
      i2c_WriteByte_MPU9250 (MPU9250_ADDRESS, ZA_OFFSET_H, data[4]);                     
      i2c_WriteByte_MPU9250 (MPU9250_ADDRESS, ZA_OFFSET_L, data[5]); */

      // Output scaled accelerometer biases for manual subtraction in the main program
      dest2[0] = (float)accel_bias[0] / (float)accelsensitivity;
      dest2[1] = (float)accel_bias[1] / (float)accelsensitivity;   
      dest2[2] = (float)accel_bias[2] / (float)accelsensitivity;       
}

Where do you advice to dig for a problem ? Thank you in advance.

kriswiner commented 7 years ago

Looks like your self test is not working properly. I would focus on this first.

TopperBG commented 7 years ago

Update to my questions. I've managed get raw data from IMU when lower I²C to 100Hz. Could be something hardware but will investigate later. The results are next: 1.Init, Self-test and Calibration of 9250

Setup MPU 9250 unit

I AM 0x71 MPU9250 is online...

Reset MPU9250

SelfTest MPU9250 x-axis self test: acceleration trim within : 56.12 % of factory value y-axis self test: acceleration trim within : 16.61 % of factory value z-axis self test: acceleration trim within : 36.62 % of factory value x-axis self test: gyration trim within : -27.10 % of factory value y-axis self test: gyration trim within : 156.94 % of factory value z-axis self test: gyration trim within : 58.03 % of factory value

Calibrating MPU:

1858 samples for avrg. accel biases (mg) -997.07 -997.07 25.69 mg gyro biases (mg) -124.72 -124.70 -124.70 o/s

If I assume right - self-test should deviate in +- 1.4% obviously not achieved. That is not first run and calculated biases should be written into registers ?

2.Init, Calibrate, Data to/from magnetometer AK8963 Pass calibration, set biases like biasX= +240.0 biasY=+199.0 biasZ=+409.0 and for my geolocation Yaw inclination yaw -= 4.8;

I AM 0x48 Compass is online...

Init AK8963 X-Axis sensitivity adjustment value 1.16 Y-Axis sensitivity adjustment value 1.16 Z-Axis sensitivity adjustment value 1.12

Set AK8963_CNTL1 to B0 Calibrate AK8963 Measurement mode 12 Mag Calibration: Wave device in a figure eight until done! ................................................................................................................................ Mag Calibration done! AK8963 mag biases (mG) 0.00 0.00 0.00 AK8963 mag scale (mG) 0.77 1.18 1.14 End MPU 9250 setup
Start ***

Obviously biases are wrong! After that ARHS parameters fluctuating very widely and I can't trust them :/ The dev board is placed on table untouched and heading north (if that matters)

Yaw=82.96 Pitch=-60.09 Roll=-160.26 27.56 degrees C Yaw=77.32 Pitch=52.17 Roll=-129.84 27.56 degrees C Yaw=-20.66 Pitch=32.51 Roll=178.30 27.61 degrees C Yaw=-123.42 Pitch=-25.98 Roll=146.80 27.56 degrees C Yaw=167.01 Pitch=14.17 Roll=174.86 27.56 degrees C Yaw=28.33 Pitch=48.87 Roll=93.51 27.61 degrees C Yaw=-7.63 Pitch=-23.25 Roll=152.06 27.51 degrees C Yaw=-72.28 Pitch=18.02 Roll=-154.14 27.70 degrees C Yaw=-175.12 Pitch=14.23 Roll=163.28 27.51 degrees C Yaw=68.37 Pitch=-27.91 Roll=174.13 27.51 degrees C Yaw=-178.51 Pitch=65.39 Roll=-118.97 27.61 degrees C Yaw=148.11 Pitch=-54.78 Roll=-178.83 27.51 degrees C Yaw=-169.08 Pitch=47.97 Roll=-133.30 27.56 degrees C Yaw=106.76 Pitch=-32.41 Roll=82.49 27.56 degrees C

Note: I don't have RTC and make data acquisition with timer every 200ms (deltaT)

kriswiner commented 7 years ago

You are not passing the self test. I would forget about everything else until you do. There looks like some kinds of integer conversion error perhaps. What is your MCU? Maybe you need to switch to something like a 32-bit Teensy?

TopperBG commented 7 years ago

MCU is Microchip PIC24FJ128GB106 and I can't switch it - it's part of complicated system with special designed board.... About self-test - If I assume right (correct me if I'm wrong) we must get trim values within 1.4% and that must be achieved after one calibration and biases is written in MPU registers ?

TopperBG commented 7 years ago

First run:

Reset MPU9250

SelfTest MPU9250 x-axis self test: acceleration trim within : -50.09 % of factory value y-axis self test: acceleration trim within : 16.48 % of factory value z-axis self test: acceleration trim within : 36.46 % of factory value x-axis self test: gyration trim within : -28.27 % of factory value y-axis self test: gyration trim within : 155.78 % of factory value z-axis self test: gyration trim within : 56.68 % of factory value

Calibrating MPU

1858 samples for avrg. accel biases (mg) 1872.80 1872.74 894.71 mg gyro biases (mg) 234.17 234.18 234.18 o/s

SelfTest MPU9250 x-axis self test: acceleration trim within : 110.19 % of factory value y-axis self test: acceleration trim within : 13.05 % of factory value z-axis self test: acceleration trim within : -2.47 % of factory value x-axis self test: gyration trim within : 114.19 % of factory value y-axis self test: gyration trim within : 155.19 % of factory value z-axis self test: gyration trim within : 184.31 % of factory value

Init MPU9250

Second run:

Reset MPU9250

SelfTest MPU9250 x-axis self test: acceleration trim within : -49.63 % of factory value y-axis self test: acceleration trim within : 15.11 % of factory value z-axis self test: acceleration trim within : 37.44 % of factory value x-axis self test: gyration trim within : -28.18 % of factory value y-axis self test: gyration trim within : 155.41 % of factory value z-axis self test: gyration trim within : 56.66 % of factory value

Calibrating MPU

1858 samples for avrg. accel biases (mg) 1872.74 1872.74 894.71 mg gyro biases (mg) 234.17 234.18 234.18 o/s

SelfTest MPU9250 x-axis self test: acceleration trim within : 110.33 % of factory value y-axis self test: acceleration trim within : 12.91 % of factory value z-axis self test: acceleration trim within : -2.47 % of factory value x-axis self test: gyration trim within : 114.17 % of factory value y-axis self test: gyration trim within : 155.17 % of factory value z-axis self test: gyration trim within : 184.27 % of factory value

Init MPU9250

kriswiner commented 7 years ago

This looks like an integer cast problem.

TopperBG commented 7 years ago

What does that means ?

kriswiner commented 7 years ago

The data from this sensor (like most) comes in the form of bytes. The bytes are in two's complement form, that is how negative integers are carried in byte form. Uusally tou can convert the bytes to the proper integer number by type casting like this:

(int16_t) MSBbyte << 8 | LSBbyte

But not all MCUs support this. So check the raw bytes coming from the data registers and verfiy that when the integers are supposed to be negative, that they are. Also, not all MCUs define int the same way, so if you are using ing or long instead of int16_t, for example, you could be truncating the integers, etc.

In any case, until you can get sensible results from the self test, everything else is a waste of time.

TopperBG commented 7 years ago

I'm pretty sure that using MSBbyte << 8 | LSBbyte (for other i2c peripherals and EEPROMs) and it works. Will make some heavy debugs at LSB and MSB from sensors to track the problem. Thanks will keep feedback.

TopperBG commented 7 years ago

Here is changed cast function and (a lot of) heavy debug. For me calculation is correct but not the results:

I AM 0x71 MPU9250 is online...

Reset MPU9250

SelfTest MPU9250 RawData0=1 RawData1=AC aAvg_X=1AC aAvg[0]=428 RawData0=1 RawData1=6F aAvg_X=16F aAvg[0]=795 RawData0=1 RawData1=2F aAvg_X=12F aAvg[0]=1098 RawData0=1 RawData1=46 aAvg_X=146 aAvg[0]=1424 RawData0=3 RawData1=57 aAvg_X=357 aAvg[0]=2279 RawData0=3 RawData1=43 aAvg_X=343 aAvg[0]=3114 RawData0=1 RawData1=98 aAvg_X=198 aAvg[0]=3522 RawData0=1 RawData1=5D aAvg_X=15D aAvg[0]=3871 RawData0=1 RawData1=E7 aAvg_X=1E7 aAvg[0]=4358 RawData0=1 RawData1=79 aAvg_X=179 aAvg[0]=4735 RawData0=1 RawData1=F2 aAvg_X=1F2 aAvg[0]=5233 RawData0=1 RawData1=D2 aAvg_X=1D2 aAvg[0]=5699 RawData0=1 RawData1=E2 aAvg_X=1E2 aAvg[0]=6181 RawData0=1 RawData1=BF aAvg_X=1BF aAvg[0]=6628 RawData0=1 RawData1=BD aAvg_X=1BD aAvg[0]=7073 RawData0=1 RawData1=C1 aAvg_X=1C1 aAvg[0]=7522 RawData0=1 RawData1=33 aAvg_X=133 aAvg[0]=7829 RawData0=1 RawData1=55 aAvg_X=155 aAvg[0]=8170 RawData0=1 RawData1=3B aAvg_X=13B aAvg[0]=8485 RawData0=1 RawData1=59 aAvg_X=159 aAvg[0]=8830 RawData0=1 RawData1=9C aAvg_X=19C aAvg[0]=9242 RawData0=1 RawData1=42 aAvg_X=142 aAvg[0]=9564 RawData0=1 RawData1=5D aAvg_X=15D aAvg[0]=9913 RawData0=1 RawData1=75 aAvg_X=175 aAvg[0]=10286 RawData0=1 RawData1=84 aAvg_X=184 aAvg[0]=10674 RawData0=1 RawData1=8F aAvg_X=18F aAvg[0]=11073 RawData0=1 RawData1=51 aAvg_X=151 aAvg[0]=11410 RawData0=1 RawData1=90 aAvg_X=190 aAvg[0]=11810 RawData0=1 RawData1=CB aAvg_X=1CB aAvg[0]=12269 RawData0=1 RawData1=7C aAvg_X=17C aAvg[0]=12649 RawData0=1 RawData1=9B aAvg_X=19B aAvg[0]=13060 RawData0=1 RawData1=BE aAvg_X=1BE aAvg[0]=13506 RawData0=1 RawData1=91 aAvg_X=191 aAvg[0]=13907 RawData0=1 RawData1=F0 aAvg_X=1F0 aAvg[0]=14403 RawData0=1 RawData1=F4 aAvg_X=1F4 aAvg[0]=14903 RawData0=1 RawData1=A8 aAvg_X=1A8 aAvg[0]=15327 RawData0=1 RawData1=8F aAvg_X=18F aAvg[0]=15726 RawData0=1 RawData1=BF aAvg_X=1BF aAvg[0]=16173 RawData0=1 RawData1=7B aAvg_X=17B aAvg[0]=16552 RawData0=1 RawData1=60 aAvg_X=160 aAvg[0]=16904 RawData0=1 RawData1=C0 aAvg_X=1C0 aAvg[0]=17352 RawData0=1 RawData1=40 aAvg_X=140 aAvg[0]=17672 RawData0=1 RawData1=C2 aAvg_X=1C2 aAvg[0]=18122 RawData0=1 RawData1=7E aAvg_X=17E aAvg[0]=18504 RawData0=1 RawData1=7A aAvg_X=17A aAvg[0]=18882 RawData0=1 RawData1=62 aAvg_X=162 aAvg[0]=19236 RawData0=1 RawData1=A0 aAvg_X=1A0 aAvg[0]=19652 RawData0=1 RawData1=7D aAvg_X=17D aAvg[0]=20033 RawData0=1 RawData1=87 aAvg_X=187 aAvg[0]=20424 RawData0=1 RawData1=6E aAvg_X=16E aAvg[0]=20790 aAvg=415 184 16958 gAvg=-139 -31 10 RawData0=1D RawData1=8E aAvg_X=1D8E aSTAvg[0]=7566 RawData0=1D RawData1=66 aAvg_X=1D66 aSTAvg[0]=15092 RawData0=1D RawData1=56 aAvg_X=1D56 aSTAvg[0]=22602 RawData0=1D RawData1=7A aAvg_X=1D7A aSTAvg[0]=30148 RawData0=1D RawData1=53 aAvg_X=1D53 aSTAvg[0]=37655 RawData0=1D RawData1=51 aAvg_X=1D51 aSTAvg[0]=45160 RawData0=1D RawData1=58 aAvg_X=1D58 aSTAvg[0]=52672 RawData0=1D RawData1=97 aAvg_X=1D97 aSTAvg[0]=60247 RawData0=1D RawData1=8A aAvg_X=1D8A aSTAvg[0]=67809 RawData0=1D RawData1=93 aAvg_X=1D93 aSTAvg[0]=75380 RawData0=1D RawData1=50 aAvg_X=1D50 aSTAvg[0]=82884 RawData0=1D RawData1=32 aAvg_X=1D32 aSTAvg[0]=90358 RawData0=1D RawData1=1F aAvg_X=1D1F aSTAvg[0]=97813 RawData0=1D RawData1=96 aAvg_X=1D96 aSTAvg[0]=105387 RawData0=1D RawData1=67 aAvg_X=1D67 aSTAvg[0]=112914 RawData0=1D RawData1=53 aAvg_X=1D53 aSTAvg[0]=120421 RawData0=1D RawData1=9F aAvg_X=1D9F aSTAvg[0]=128004 RawData0=1D RawData1=79 aAvg_X=1D79 aSTAvg[0]=135549 RawData0=1D RawData1=76 aAvg_X=1D76 aSTAvg[0]=143091 RawData0=1D RawData1=5A aAvg_X=1D5A aSTAvg[0]=150605 RawData0=1D RawData1=97 aAvg_X=1D97 aSTAvg[0]=158180 RawData0=1D RawData1=48 aAvg_X=1D48 aSTAvg[0]=165676 RawData0=1D RawData1=65 aAvg_X=1D65 aSTAvg[0]=173201 RawData0=1D RawData1=B7 aAvg_X=1DB7 aSTAvg[0]=180808 RawData0=1D RawData1=82 aAvg_X=1D82 aSTAvg[0]=188362 RawData0=1D RawData1=24 aAvg_X=1D24 aSTAvg[0]=195822 RawData0=1D RawData1=52 aAvg_X=1D52 aSTAvg[0]=203328 RawData0=1D RawData1=4E aAvg_X=1D4E aSTAvg[0]=210830 RawData0=1D RawData1=3A aAvg_X=1D3A aSTAvg[0]=218312 RawData0=1D RawData1=49 aAvg_X=1D49 aSTAvg[0]=225809 RawData0=1D RawData1=1B aAvg_X=1D1B aSTAvg[0]=233260 RawData0=1D RawData1=34 aAvg_X=1D34 aSTAvg[0]=240736 RawData0=1D RawData1=6F aAvg_X=1D6F aSTAvg[0]=248271 RawData0=1D RawData1=79 aAvg_X=1D79 aSTAvg[0]=255816 RawData0=1D RawData1=81 aAvg_X=1D81 aSTAvg[0]=263369 RawData0=1D RawData1=C7 aAvg_X=1DC7 aSTAvg[0]=270992 RawData0=1D RawData1=A6 aAvg_X=1DA6 aSTAvg[0]=278582 RawData0=1D RawData1=49 aAvg_X=1D49 aSTAvg[0]=286079 RawData0=1D RawData1=30 aAvg_X=1D30 aSTAvg[0]=293551 RawData0=1D RawData1=80 aAvg_X=1D80 aSTAvg[0]=301103 RawData0=1D RawData1=41 aAvg_X=1D41 aSTAvg[0]=308592 RawData0=1D RawData1=44 aAvg_X=1D44 aSTAvg[0]=316084 RawData0=1D RawData1=79 aAvg_X=1D79 aSTAvg[0]=323629 RawData0=1D RawData1=94 aAvg_X=1D94 aSTAvg[0]=331201 RawData0=1D RawData1=7D aAvg_X=1D7D aSTAvg[0]=338750 RawData0=1D RawData1=81 aAvg_X=1D81 aSTAvg[0]=346303 RawData0=1D RawData1=4E aAvg_X=1D4E aSTAvg[0]=353805 RawData0=1D RawData1=5E aAvg_X=1D5E aSTAvg[0]=361323 RawData0=1D RawData1=79 aAvg_X=1D79 aSTAvg[0]=368868 RawData0=1D RawData1=75 aAvg_X=1D75 aSTAvg[0]=376409 aSTAvg=7528 6517 25466 gSTAvg=18580 22237 24814 x-axis self test: acceleration trim within : 14.23 % of factory value y-axis self test: acceleration trim within : -75.72 % of factory value z-axis self test: acceleration trim within : -2.58 % of factory value x-axis self test: gyration trim within : 17.98 % of factory value y-axis self test: gyration trim within : -17.16 % of factory value z-axis self test: gyration trim within : 410.85 % of factory value

Calibrating MPU FIFO_H 1 FIFO_L 74

31 samples for avrg. Data0=1 Data1=AC RawCalib_X=1AC Calib_X[0]=428 Data0=1 Data1=B0 RawCalib_X=1B0 Calib_X[0]=432 Data0=1 Data1=74 RawCalib_X=174 Calib_X[0]=372 Data0=1 Data1=48 RawCalib_X=148 Calib_X[0]=328 Data0=1 Data1=5C RawCalib_X=15C Calib_X[0]=348 Data0=1 Data1=6C RawCalib_X=16C Calib_X[0]=364 Data0=1 Data1=54 RawCalib_X=154 Calib_X[0]=340 Data0=1 Data1=30 RawCalib_X=130 Calib_X[0]=304 Data0=1 Data1=64 RawCalib_X=164 Calib_X[0]=356 Data0=1 Data1=74 RawCalib_X=174 Calib_X[0]=372 Data0=1 Data1=68 RawCalib_X=168 Calib_X[0]=360 Data0=1 Data1=70 RawCalib_X=170 Calib_X[0]=368 Data0=1 Data1=84 RawCalib_X=184 Calib_X[0]=388 Data0=1 Data1=6C RawCalib_X=16C Calib_X[0]=364 Data0=1 Data1=5C RawCalib_X=15C Calib_X[0]=348 Data0=1 Data1=9C RawCalib_X=19C Calib_X[0]=412 Data0=1 Data1=90 RawCalib_X=190 Calib_X[0]=400 Data0=1 Data1=48 RawCalib_X=148 Calib_X[0]=328 Data0=1 Data1=4C RawCalib_X=14C Calib_X[0]=332 Data0=1 Data1=38 RawCalib_X=138 Calib_X[0]=312 Data0=1 Data1=3C RawCalib_X=13C Calib_X[0]=316 Data0=1 Data1=78 RawCalib_X=178 Calib_X[0]=376 Data0=1 Data1=A4 RawCalib_X=1A4 Calib_X[0]=420 Data0=1 Data1=A4 RawCalib_X=1A4 Calib_X[0]=420 Data0=1 Data1=54 RawCalib_X=154 Calib_X[0]=340 Data0=1 Data1=74 RawCalib_X=174 Calib_X[0]=372 Data0=1 Data1=A4 RawCalib_X=1A4 Calib_X[0]=420 Data0=1 Data1=6C RawCalib_X=16C Calib_X[0]=364 Data0=1 Data1=48 RawCalib_X=148 Calib_X[0]=328 Data0=1 Data1=58 RawCalib_X=158 Calib_X[0]=344 Data0=1 Data1=34 RawCalib_X=134 Calib_X[0]=308 accel_bias[0] 363 accel_bias[1] 114 accel_bias[2] 10670 gyro_bias[0] 363 gyro_bias[1] 114 gyro_bias[2] 10670 Data0=F1 Data1=62 RawBias_X=F162 accel_bias_reg[0]=61794 accel biases (mg) 22.15 6.95 -348.75 mg gyro biases (mg) -0.77 -0.04 -0.05 o/s

SelfTest MPU9250 RawData0=FC RawData1=E4 aAvg_X=FCE4 aAvg[0]=-796 RawData0=FC RawData1=B2 aAvg_X=FCB2 aAvg[0]=-1642 RawData0=FC RawData1=E8 aAvg_X=FCE8 aAvg[0]=-2434 RawData0=FC RawData1=C1 aAvg_X=FCC1 aAvg[0]=-3265 RawData0=FC RawData1=B9 aAvg_X=FCB9 aAvg[0]=-4104 RawData0=FC RawData1=D2 aAvg_X=FCD2 aAvg[0]=-4918 RawData0=FC RawData1=BE aAvg_X=FCBE aAvg[0]=-5752 RawData0=FC RawData1=E2 aAvg_X=FCE2 aAvg[0]=-6550 RawData0=FC RawData1=B1 aAvg_X=FCB1 aAvg[0]=-7397 RawData0=FC RawData1=AB aAvg_X=FCAB aAvg[0]=-8250 RawData0=FC RawData1=FA aAvg_X=FCFA aAvg[0]=-9024 RawData0=FC RawData1=D5 aAvg_X=FCD5 aAvg[0]=-9835 RawData0=FC RawData1=E3 aAvg_X=FCE3 aAvg[0]=-10632 RawData0=FC RawData1=DB aAvg_X=FCDB aAvg[0]=-11437 RawData0=FC RawData1=A9 aAvg_X=FCA9 aAvg[0]=-12292 RawData0=FC RawData1=C8 aAvg_X=FCC8 aAvg[0]=-13116 RawData0=FC RawData1=BB aAvg_X=FCBB aAvg[0]=-13953 RawData0=FC RawData1=D0 aAvg_X=FCD0 aAvg[0]=-14769 RawData0=FC RawData1=E2 aAvg_X=FCE2 aAvg[0]=-15567 RawData0=FC RawData1=B2 aAvg_X=FCB2 aAvg[0]=-16413 RawData0=FC RawData1=A7 aAvg_X=FCA7 aAvg[0]=-17270 RawData0=FC RawData1=DC aAvg_X=FCDC aAvg[0]=-18074 RawData0=FC RawData1=C2 aAvg_X=FCC2 aAvg[0]=-18904 RawData0=FD RawData1=6 aAvg_X=FD06 aAvg[0]=-19666 RawData0=FC RawData1=CA aAvg_X=FCCA aAvg[0]=-20488 RawData0=FD RawData1=43 aAvg_X=FD43 aAvg[0]=-21189 RawData0=FC RawData1=50 aAvg_X=FC50 aAvg[0]=-22133 RawData0=FC RawData1=D3 aAvg_X=FCD3 aAvg[0]=-22946 RawData0=FC RawData1=75 aAvg_X=FC75 aAvg[0]=-23853 RawData0=FC RawData1=CC aAvg_X=FCCC aAvg[0]=-24673 RawData0=FC RawData1=DF aAvg_X=FCDF aAvg[0]=-25474 RawData0=FC RawData1=B8 aAvg_X=FCB8 aAvg[0]=-26314 RawData0=FC RawData1=9B aAvg_X=FC9B aAvg[0]=-27183 RawData0=FC RawData1=C3 aAvg_X=FCC3 aAvg[0]=-28012 RawData0=FC RawData1=BA aAvg_X=FCBA aAvg[0]=-28850 RawData0=FC RawData1=C9 aAvg_X=FCC9 aAvg[0]=-29673 RawData0=FC RawData1=DE aAvg_X=FCDE aAvg[0]=-30475 RawData0=FC RawData1=A6 aAvg_X=FCA6 aAvg[0]=-31333 RawData0=FD RawData1=26 aAvg_X=FD26 aAvg[0]=-32063 RawData0=FC RawData1=C5 aAvg_X=FCC5 aAvg[0]=-32890 RawData0=FC RawData1=D9 aAvg_X=FCD9 aAvg[0]=-33697 RawData0=FC RawData1=AF aAvg_X=FCAF aAvg[0]=-34546 RawData0=FC RawData1=BE aAvg_X=FCBE aAvg[0]=-35380 RawData0=FC RawData1=8F aAvg_X=FC8F aAvg[0]=-36261 RawData0=FC RawData1=BA aAvg_X=FCBA aAvg[0]=-37099 RawData0=FC RawData1=BD aAvg_X=FCBD aAvg[0]=-37934 RawData0=FC RawData1=C2 aAvg_X=FCC2 aAvg[0]=-38764 RawData0=FC RawData1=BC aAvg_X=FCBC aAvg[0]=-39600 RawData0=FC RawData1=E6 aAvg_X=FCE6 aAvg[0]=-40394 RawData0=FC RawData1=E2 aAvg_X=FCE2 aAvg[0]=-41192 aAvg=-823 11 22689 gAvg=16 -8 -14 RawData0=18 RawData1=C1 aAvg_X=18C1 aSTAvg[0]=6337 RawData0=18 RawData1=BF aAvg_X=18BF aSTAvg[0]=12672 RawData0=18 RawData1=F1 aAvg_X=18F1 aSTAvg[0]=19057 RawData0=17 RawData1=19 aAvg_X=1719 aSTAvg[0]=24970 RawData0=17 RawData1=FF aAvg_X=17FF aSTAvg[0]=31113 RawData0=18 RawData1=C0 aAvg_X=18C0 aSTAvg[0]=37449 RawData0=19 RawData1=2A aAvg_X=192A aSTAvg[0]=43891 RawData0=18 RawData1=A5 aAvg_X=18A5 aSTAvg[0]=50200 RawData0=19 RawData1=11 aAvg_X=1911 aSTAvg[0]=56617 RawData0=18 RawData1=D4 aAvg_X=18D4 aSTAvg[0]=62973 RawData0=18 RawData1=CB aAvg_X=18CB aSTAvg[0]=69320 RawData0=18 RawData1=88 aAvg_X=1888 aSTAvg[0]=75600 RawData0=18 RawData1=DA aAvg_X=18DA aSTAvg[0]=81962 RawData0=18 RawData1=A4 aAvg_X=18A4 aSTAvg[0]=88270 RawData0=18 RawData1=AD aAvg_X=18AD aSTAvg[0]=94587 RawData0=18 RawData1=BB aAvg_X=18BB aSTAvg[0]=100918 RawData0=18 RawData1=DF aAvg_X=18DF aSTAvg[0]=107285 RawData0=18 RawData1=A6 aAvg_X=18A6 aSTAvg[0]=113595 RawData0=18 RawData1=F3 aAvg_X=18F3 aSTAvg[0]=119982 RawData0=18 RawData1=77 aAvg_X=1877 aSTAvg[0]=126245 RawData0=18 RawData1=9E aAvg_X=189E aSTAvg[0]=132547 RawData0=18 RawData1=76 aAvg_X=1876 aSTAvg[0]=138809 RawData0=18 RawData1=CE aAvg_X=18CE aSTAvg[0]=145159 RawData0=18 RawData1=82 aAvg_X=1882 aSTAvg[0]=151433 RawData0=18 RawData1=82 aAvg_X=1882 aSTAvg[0]=157707 RawData0=18 RawData1=B2 aAvg_X=18B2 aSTAvg[0]=164029 RawData0=18 RawData1=AA aAvg_X=18AA aSTAvg[0]=170343 RawData0=18 RawData1=CE aAvg_X=18CE aSTAvg[0]=176693 RawData0=18 RawData1=B5 aAvg_X=18B5 aSTAvg[0]=183018 RawData0=18 RawData1=BD aAvg_X=18BD aSTAvg[0]=189351 RawData0=18 RawData1=CB aAvg_X=18CB aSTAvg[0]=195698 RawData0=17 RawData1=F3 aAvg_X=17F3 aSTAvg[0]=201829 RawData0=18 RawData1=75 aAvg_X=1875 aSTAvg[0]=208090 RawData0=18 RawData1=C7 aAvg_X=18C7 aSTAvg[0]=214433 RawData0=18 RawData1=B9 aAvg_X=18B9 aSTAvg[0]=220762 RawData0=18 RawData1=AD aAvg_X=18AD aSTAvg[0]=227079 RawData0=18 RawData1=E6 aAvg_X=18E6 aSTAvg[0]=233453 RawData0=18 RawData1=C9 aAvg_X=18C9 aSTAvg[0]=239798 RawData0=18 RawData1=9F aAvg_X=189F aSTAvg[0]=246101 RawData0=18 RawData1=CF aAvg_X=18CF aSTAvg[0]=252452 RawData0=18 RawData1=A2 aAvg_X=18A2 aSTAvg[0]=258758 RawData0=18 RawData1=BA aAvg_X=18BA aSTAvg[0]=265088 RawData0=18 RawData1=CC aAvg_X=18CC aSTAvg[0]=271436 RawData0=18 RawData1=BD aAvg_X=18BD aSTAvg[0]=277769 RawData0=18 RawData1=A6 aAvg_X=18A6 aSTAvg[0]=284079 RawData0=18 RawData1=A5 aAvg_X=18A5 aSTAvg[0]=290388 RawData0=18 RawData1=87 aAvg_X=1887 aSTAvg[0]=296667 RawData0=18 RawData1=DA aAvg_X=18DA aSTAvg[0]=303029 RawData0=18 RawData1=B6 aAvg_X=18B6 aSTAvg[0]=309355 RawData0=18 RawData1=D4 aAvg_X=18D4 aSTAvg[0]=315711 aSTAvg=6314 6384 31205 gSTAvg=18666 22237 24797 x-axis self test: acceleration trim within : -78.24 % of factory value y-axis self test: acceleration trim within : -55.62 % of factory value z-axis self test: acceleration trim within : -74.03 % of factory value x-axis self test: gyration trim within : -41.42 % of factory value y-axis self test: gyration trim within : -32.18 % of factory value z-axis self test: gyration trim within : -22.84 % of factory value

Also I PM you for self-test documentation regarding factoryTrim formulas but don't have responce yet ?

TopperBG commented 7 years ago

One more note (I've just self replay, odd) Is that values from Self-test normal? So big difference from real raw data (not in test mode)

 aAvg=592 32 16928
 gAvg=41 -76 196
 aSTAvg=7790 6474 25475
 gSTAvg=18590 22134 24825