Obtaining biases as a result of gyroscope and accelerometer calibration

Projects Programming
1

Rewriting the code of a simple function

MPU9250/Dual_MPU9250/MPU9250.cpp at 72038b040bef3cf072612cd8a8ee8f26e3c87158 · kriswiner/MPU9250 · GitHub

For sensors
lsm303 and l3g20h
Code below

#define GYRO_RANGE 2000.0f
#define ACCEL_RANGE 8.0f

// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
void calibrateSensor(float * dest1, float * dest2)
{  
  uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
  uint16_t ii, packet_count, fifo_count;
  int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
 
  // uint16_t  gyrosensitivity  = 131;   // = 131 LSB/degrees/sec
  // uint16_t  accelsensitivity = 16384;  // = 16384 LSB/g
  uint16_t  gyrosensitivity  = GYRO_RANGE;
  uint16_t  accelsensitivity = ACCEL_RANGE;

  fifo_count = 100;
  packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging

  for (ii = 0; ii < packet_count; ii++) {
    int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};

    // Form signed 16-bit integer for each sample in FIFO
    accel_temp[0] = (int16_t) lsm303.accelData.x;
    accel_temp[1] = (int16_t) lsm303.accelData.y;
    accel_temp[2] = (int16_t) lsm303.accelData.z;    
    gyro_temp[0]  = (int16_t) gyro.g.x;
    gyro_temp[1]  = (int16_t) gyro.g.y;
    gyro_temp[2]  = (int16_t) gyro.g.z;
    
    // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
    accel_bias[0] += (int32_t) accel_temp[0];
    accel_bias[1] += (int32_t) accel_temp[1];
    accel_bias[2] += (int32_t) accel_temp[2];
    gyro_bias[0]  += (int32_t) gyro_temp[0];
    gyro_bias[1]  += (int32_t) gyro_temp[1];
    gyro_bias[2]  += (int32_t) gyro_temp[2];
            
}
    // Normalize sums to get average count biases
    accel_bias[0] /= (int32_t) packet_count;
    accel_bias[1] /= (int32_t) packet_count;
    accel_bias[2] /= (int32_t) packet_count;
    gyro_bias[0]  /= (int32_t) packet_count;
    gyro_bias[1]  /= (int32_t) packet_count;
    gyro_bias[2]  /= (int32_t) packet_count;
    
  if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;}  // Remove gravity from the z-axis accelerometer bias calculation
  else {accel_bias[2] += (int32_t) accelsensitivity;}

// Push gyro biases to hardware registers

  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;

// 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;
}

A small digression - I tried to subtract from the sensors via FIFO, as in the example

https://forum.pololu.com/t/lsm-303-fifo-mode-programming/6975

But the reading occurred only 4-6 values ​​and then zeros.

Setup Done
STatus A : 255 RefA 7 Count : 7
STatus A : 255 RefA 142 Count : 14
STatus A : 11 RefA 145 Count : 17
STatus A : 15 RefA 149 Count : 21
STatus A : 137 RefA 154 Count : 26
STatus A : 137 RefA 157 Count : 29
STatus A : 15 RefA 0 Count : 0
STatus A : 255 RefA 0 Count : 0
STatus A : 255 RefA 0 Count : 0

Therefore, I decided not to use FIFO.
The function I rewrote
calibrateSensor(gyroBias, accelBias);
At the moment the MSU starts, it produces the following values:

Accel + mag LSM303 FOUND!
Gyro L3DG20 FOUND!
gyroBias[0]: 0.09
gyroBias[1]: -0.30
gyroBias[2]: -0.41
accelBias[0]: 0.09
accelBias[1]: -58.00
accelBias[2]: 431.00

What confuses me is that gyro_bias[0] and accel_bias[0] are the same, although they do not intersect in any way in the function code.
What am I doing wrong, please tell me?

2

I'm sorry, but it looks like I posted the wrong topic.
I found the reason for outputting identical values ​​to the serial port after posting this post.
The reason is banal - my carelessness!
We need to finish writing code without stopping)))
Section of my code for output

 #ifdef DEBUG

 Serial.print("gyroBias[0]: ");
 Serial.println(gyroBias[0]);
 Serial.print("gyroBias[1]: ");
 Serial.println(gyroBias[1]);
 Serial.print("gyroBias[2]: ");
 Serial.println(gyroBias[2]);

 Serial.print("gyroBias[0]: ");
 Serial.println(accelBias[0]);
 Serial.print("accelBias[1]: ");
 Serial.println(accelBias[1]);
 Serial.print("accelBias[2]: ");
 Serial.println(accelBias[2]);

 delay(2000);

 #endif

Accel + mag LSM303 FOUND!
Gyro L3DG20 FOUND!
gyroBias[0]: -0.07
gyroBias[1]: 0.42
gyroBias[2]: 0.35
accelBias[0]: -8.00
accelBias[1]: -6.00
accelBias[2]: 427.00

I think I’ll leave this post as a cheat sheet, in case someone finds the function of calculating offsets useful.