Electronic – dspic MPU6050 calibration problem

imupic

I use dspic33fj128gp802 and MPU6050 for my project. Microcontroller succesfully takes the data from sensors 6 channel and sends the data pc via UART. When i want to add "float 2.0" or "int 2" to accelerometer data for calibration it stops to send data to uart. I've tried all of data type combinations but it always fails. I only add one line of code to my source code but when i add, the code doesn't send data to PC. I use MikroC pro for dspic. The newly added line can be seen in source code as bold in function of Extract_Readings. This newly added line does not generate any compiling or debugging error.

accel_z = (float) accel_z + 2.0;

As a solution when i change the last line to accel_z = (float) accel_z / 2.0;

it succesfully divides the result to 2. As a summary, dividing operator works well but addition operator does not work.

I cannot find any solution. I am waiting for your considerations.

Thank you.

The source code is below:

 #include "MPU_IMU_Register_Map.h"

unsigned int raw_data[7];

sbit INT at RB7_bit;
sbit INT_Direction at TRISB7_bit;

int gyro_x_temp, gyro_y_temp, gyro_z_temp, accel_x_temp, accel_y_temp, accel_z_temp, temp_raw;
char i, buff, gyro_x_out[15], gyro_y_out[15], gyro_z_out[15], accel_x_out[15], accel_y_out[15], accel_z_out[15], temp_out[15];
float temp, gyro_x, gyro_y, gyro_z, accel_x, accel_y, accel_z;

void MCU_Init(){
  ADPCFG = 0xffff;

  TRISBbits.TRISB10 = 1;
  TRISBbits.TRISB11 = 0;
  Delay_ms(100);

  RPINR18bits.U1RXR = 10;
  RPOR5bits.RP11R   = 3;

  INT_Direction = 1;
  I2C1_Init(400000);
  Delay_ms(100);

  Delay_ms(100);
  UART1_Init_Advanced(19200, 1, 1, 1);
  Delay_ms(600);

  UART1_Write_Text("MCU Ready\n");
  Delay_ms(100);
}
void MPU_I2C_Write(unsigned char s_addr, unsigned char r_addr, unsigned char len, unsigned char *dat) {
 unsigned int i;
 I2C1_Start();                         // issue I2C start signal
 I2C1_Write(s_addr & 0xFE);            // send byte via I2C  (device address + W(&0xFE))
 I2C1_Write(r_addr);                   // send byte (address of EEPROM location)
 for (i = 0 ; i < len ; i++){
  I2C1_Write(*dat++);                 // send data (data to be written)
 }
 I2C1_Stop();                          // issue I2C stop signal

}

void MPU_I2C_Read(unsigned char s_addr, unsigned char r_addr, unsigned char len, unsigned char *dat) {
 unsigned int i;
 I2C1_Start();                         // issue I2C start signal
 I2C1_Write(s_addr & 0xFE);            // send byte via I2C  (device address + W(&0xFE))
 I2C1_Write(r_addr);                   // send byte (data address)
 I2C1_Restart();                       // issue I2C signal repeated start
 I2C1_Write(s_addr | 0x01);            // send byte (device address + R(|0x01))
 for (i = 0; i < (len-1); i++){
  *dat++ = I2C1_Read(_I2C_ACK);       // Read the data (acknowledge)
  }
  *dat = I2C1_Read(_I2C_NACK);          // Read the data (NO acknowledge)
 I2C1_Stop();                          // issue I2C stop signal
 }

void MPU_I2C_Read_Int(unsigned char s_addr, unsigned char r_addr, unsigned char len, unsigned char *dat) {
  unsigned int i;
  unsigned short *pt;
  I2C1_Start();                         // issue I2C start signal
  I2C1_Write(s_addr & 0xFE);            // send byte via I2C  (device address + W(&0xFE))
  I2C1_Write(r_addr);                   // send byte (data address)
  I2C1_Restart();                       // issue I2C signal repeated start
  I2C1_Write(s_addr | 0x01);            // send byte (device address + R(|0x01))
  for (i = 0 ; i < ((len << 1)-1) ; i++){
   if (i%2) {
     pt = pt - 1;
   }
   else {
     pt = dat + i + 1;
   }
    *pt = I2C1_Read(_I2C_ACK);          // Read the data (acknowledge)
  }
  *(pt - 1) = I2C1_Read(_I2C_NACK);     // Read the data (NO acknowledge)
  I2C1_Stop();                          // issue I2C stop signal
}


void MPU_Init(){
 MPU_I2C_Write(mpu_I2C_ADDR, mpu_rm_PWR_MGMT_1 , 1, 0x80);
 Delay_ms(100);
 MPU_I2C_Write(mpu_I2C_ADDR, mpu_rm_PWR_MGMT_1 , 1, 0x00);
 Delay_ms(100);

 MPU_I2C_Write(mpu_I2C_ADDR, mpu_rm_FIFO_EN , 1, 0x78);
 Delay_ms(100);

 MPU_I2C_Write(mpu_I2C_ADDR, mpu_rm_INT_ENABLE , 1, 0x10);
 Delay_ms(100);

 MPU_I2C_Read (mpu_I2C_ADDR, mpu_rm_ACCEL_CONFIG, 1, &raw_data);
 Delay_ms(100);


 MPU_I2C_Read (mpu_I2C_ADDR, mpu_rm_INT_ENABLE, 1, &raw_data);
 Delay_ms(100);

 raw_data[0] |= 0x11;
 MPU_I2C_Write(mpu_I2C_ADDR, mpu_rm_INT_ENABLE, 1, &raw_data[0]);
 Delay_ms(100);
 UART1_Write_Text("MPU6050 Hazir\n");

}
void Extract_Readings() {

 accel_x_temp = raw_data[0];
 accel_x = (float)accel_x_temp / 2048.0;
 accel_y_temp = raw_data[1];
 accel_y = (float)accel_y_temp / 2048.0;
 accel_z_temp = raw_data[2];
 accel_z = (float)accel_z_temp / 2048.0;
 **accel_z = (float) accel_z + 2.0;** //When this line deleted or changed to accel_z = (float) accel_z /2.0, code works well

 temp_raw = raw_data[3];

 temp = (float)temp_raw / 325.0;

 gyro_x_temp = raw_data[4];
 gyro_x = (float)gyro_x_temp / 131.0;
 gyro_y_temp = raw_data[5];
 gyro_y = (float)gyro_y_temp / 131.0;

 gyro_z_temp = raw_data[6];
 gyro_z = (float)gyro_z_temp / 131.0;

}
void Convert_Readings_To_String() {

FloatToStr(accel_x, accel_x_out);
FloatToStr(accel_y, accel_y_out);
FloatToStr(accel_z, accel_z_out);


FloatToStr(temp, temp_out);


FloatToStr(gyro_x, gyro_x_out);
FloatToStr(gyro_y, gyro_y_out);
FloatToStr(gyro_z, gyro_z_out);

}


void Print_Readings() {

 Delay_ms(500);
 UART1_Write_Text("\nGyro readings:\n");
 UART1_Write(0x0D);
 UART1_Write_Text("\nx = \n");
 UART1_Write_Text(gyro_x_out);
 UART1_Write(0x0D);
 UART1_Write_Text("\ny = \n");
 UART1_Write_Text(gyro_y_out);
 UART1_Write(0x0D);
 UART1_Write_Text("\nz = \n");
 UART1_Write_Text(gyro_z_out);
 UART1_Write(0x0D);
 UART1_Write(0x0D);
 UART1_Write(0x0D);


 UART1_Write_Text("\nAccel readings:\n");
 UART1_Write(0x0D);
 UART1_Write_Text("\nx = \n");
 UART1_Write_Text(accel_x_out);
 UART1_Write(0x0D);
 UART1_Write_Text("\ny = \n");
 UART1_Write_Text(accel_y_out);
 UART1_Write(0x0D);
 UART1_Write_Text("\nz = \n");
 UART1_Write_Text(accel_z_out);
 UART1_Write(0x0D);
 UART1_Write(0x0D);
 UART1_Write(0x0D);


 UART1_Write_Text("\nTemperature readings (Celsius) = ");
 UART1_Write_Text(temp_out);
 UART1_Write(0x0D);
 UART1_Write(0x0D);
 UART1_Write(0x0D);
 Delay_ms(1000);


}
void main() {

 MCU_Init();
 MPU_Init();
 while(1) {
 while(INT != 1)
  ;
 MPU_I2C_Read_Int(mpu_I2C_ADDR, mpu_rm_ACCEL_XOUT_H, 7, &raw_data);
 Extract_Readings();
 Convert_Readings_To_String();
 Print_Readings();
}


}

Best Answer

The problem you are having is floating point precision. As a rule of thumb, DON'T use floating points if you can avoid it.

Plenty has been written about why not to use them, so I won't go into too much detail. See link.

What you are probably getting is rounding errors and most likely running out of stack and heap. For example 0.1, the simplest decimal number you can think of, looks like this in 32bit binary:

0.1 (Most accurate representation) = 1.00000001490116119384765625E-1e
Exponent        Mantessa
01111011        10011001100110011001101

In reality this number continues rounding to infinity.

Simple solution, DON'T use floats, just simply multiply all your sensor reading by 1000 to get rid of the decimal and read them in Milli precision.

Related Solutions

Electronic – Magnetometer dynamic calibration

I like your graphs. They clearly show that roll, pitch, and yaw seem to be working. Congratulations! That's already more progress than most people make.

I'm guessing that the code you presented is calculating "the wrong" MAG_Heading value, different from the MAG_Heading value you expected.

It would be a lot easier for us to help you if you gave us: (This is the "describe the symptoms" section of "How To Ask Questions The Smart Way" )

the AK8975 magnetometer output values m_x, m_y, and m_z at some single point in time.
The pitch and roll values at the same instant
the allegedly wrong MAG_Heading output value calculated from those values
what you expected the correct MAG_Heading to be

So I'm left to speculate that perhaps you're running into the same sorts of problems I create for myself :-).

What angle format do your sin() and cos() and atan2() functions expect? Do you need to do some sort of conversion between the format pitch and roll are stored in to that format? Do you need to convert from that format to what you need MAG_heading? (brads, degrees, or radians? floating-point or fixed-point?)
Is there an offset in the raw m_x, m_y, m_z values that needs to be subtracted off?
Are all the parts lined up in the way assumed by the code? In particular, is the pitch and roll axis lined up with the magnetometer axis? (Is m_x supposed to point forward, along the roll axis? Is m_y supposed to point to the right, along the pitch axis?)
Maybe some sensor value or another -- perhaps m_z -- needs to be negated before feeding into this code?
Is maybe this code being interrupted by one interrupt or another that corrupts its internal values? I seem to recall a different project that, after someone put a "divide" in an interrupt routine, every trig function calculation everywhere else in the program would often give the wrong result.
Is maybe interrupts firing so often that this code never actually finishes running?

There seems to be other people discussing very similar code elsewhere: http://diydrones.com/forum/topics/heading-from-3d-magnetometer ; http://diydrones.ning.com/profiles/blogs/dcm-imu-theory-first-draft ; http://aeroquad.com/showthread.php?1138-REVOLUTION!!!-New-IMU!!! ; http://www.rcgroups.com/forums/showthread.php?t=1436742&page=6 ; http://aeroquad.com/showthread.php?691-Hold-your-heading-with-HMC5843-Magnetometer ; etc.

Electronic – Magnetometer ∞ shaped calibration

The 8/S shaped pattern is used to calibrate magnetometers in mobile phones and other devices.

Background

Typical mobile phone era magnetometers measure the magnetic field strength along three orthogonal axes, e.g.:

\$\textbf{m} = m_x\boldsymbol{\hat{\imath}} + m_y\boldsymbol{\hat{\jmath}} + m_z\boldsymbol{\hat{k}}\$

With the magnitude of the field given by,

\$\|\textbf{m}\| = \sqrt{m_x^2 +m_y^2 + m_z^2}\$

and the angle of rotation from each axis as

\$ \theta_k = \cos^{-1} \frac{m_k}{ \| \textbf{m} \| }, \text{ where } k \in [x,y,z] \$

Calibration

Since the magenetic field of the earth is relatively constant, the magnitude of the as field measured by the magnetometer should also be constant, regardless of the orientation of the sensor. i.e. if one were to rotate the sensor around and plot \$m_x\$, \$m_y\$, and \$m_z\$ in 3D, the paths should plot out the surface of a sphere with constant radius.

Ideally it should look somthing like this:

sphere

However due to hard and soft iron effects and other distortions, it ends up looking like a deformed sphere:

deformed

This is because the magnitude of the magnetic field as measured by the sensor is changing with orientation. The result being that the direction of the magnetic field when calculated according to the formulas above is different from the true direction.

Calibration must be performed to adjust each of the three axis readings so that the magnitude is constant regardless of orientation - you can think of it as the deformed sphere must be warped into a perfect sphere. The LSM303 application note has lots of detailed instructions on how to perform this.

So what about the figure 8 pattern!?

Performing the figure 8 pattern 'traces out' part of the deformed sphere above. From the coordinates obtained, the deformation of the sphere can be estimated, and the calibration coefficients obtained. A good pattern is one that traces through the greatest range of orientations and therefore estimates the greatest deviation from the true constant magnitude.

To estimate the shape of the deformed sphere, least squares ellipse fitting can be used. The LSM303 application note also has information on this.

A simple method for a basic calibration

According to the app note if you assume no soft-iron distortion, the deformed sphere will not be tilted. Therefore a simple method for a basic calibration may be possible:

Find the maximum and minimum value for each axis, and get the 1/2 range and zero point

\$r_k = \tfrac{1}{2} (\max(m_k) - \min(m_k))\$

\$z_k = \max(m_k) - r_k\$

Shift and scale each axis measurement

\$m_k' = \frac{m_k - z_k}{r_k}\$

Calculate values as before except using \$m_k'\$

This is based off the code found here.

Solving using least squares

MATLAB code to solve using least squares is shown below. The code assumes a variable mag where the columns are the x y z values.

H = [mag(:,1), mag(:,2), mag(:,3), - mag(:,2).^2, - mag(:,3).^2, ones(size(mag(:,1)))];
w = mag(:,1).^2;
X = (H'*H)\H'*w;
offX = X(1)/2;
offY = X(2)/(2*X(4));
offZ = X(3)/(2*X(5));
temp = X(6) + offX^2 + X(4)*offY^2 + X(5)*offZ^2;
scaleX = sqrt(temp);
scaleY = sqrt(temp / X(4));
scaleZ= sqrt(temp / X(5));

To do a dynamic figure 8 calibration, you could run the least squares routine with every new reading and terminate when the offset and scale factors have stabilised.

Earth's Magnetic Field

Note, the Earth's magnetic field is usually not parallel to the surface and there may be a large down component.

Best Answer

Related Solutions

Electronic – Magnetometer dynamic calibration

Electronic – Magnetometer ∞ shaped calibration

Related Topic