Tool/software: Code Composer Studio
Hi All,
I am currently working on a project with Piccolo TMS320F28027 and trying to interface a 10 Axis IMU (GY-86) breakout board, the board has MPU6050+ HMC5883LC+MS5611 sensors that be connected via the I2C bus. I am able to talk to the sensor and get the sensor :) i am also able to run a data fusion on Accelerometer and Gyroscope to calculate the roll and pitch angles. I am also able to run a tilt compensation algorithm on the magnetometer to calculate the heading...
All this is in inside a cpu timer every 2 msec. I am invoking the timer every 2 msec which fetches the sensor data using I2C at 400 kHz the running intensive mathematical operations lke bunch of divisions, multiplications and some trigonometric operations...so I was curious about the execution time so i set a GPIO high at the start of the operation and set it low at the end....Using an oscilloscope i measured the pulse width and to my surprise the pulse width is just 20-40 us. Is this true ?
Or am i making a mistake ?
I also cross checked using break points and tried measuring the clock cycles using CCS and the clock display showed around 450,000 clock cycles so i am really confused on which is true...
All the operations are done using fixed point library based on IQ math and the code is running on flash memory...
Could someone please give me some suggestions about this ? void A1 does the data handling and calculation of the angles.....
Please find below the code:
void A0(void)
{
// loop rate synchronizer for A-tasks
if(CpuTimer2Regs.TCR.bit.TIF == 1)
{
CpuTimer2Regs.TCR.bit.TIF = 1; // clear flag
//-----------------------------------------------------------
(*A_Task_Ptr)(); // jump to an A Task (A1,A2,A3,...)
//-----------------------------------------------------------
}
}
void A1(void)
//--------------------------------------------------------
{
_iq roll,pitch, cosRoll, sinRoll, cosPitch, sinPitch, Xh, Yh;
interruptCount++;
GPIO_setLow(myGpio, GPIO_Number_0); //start of the code
accel_x=I2CA_Read2Bytes(MPU6050_RA_ACCEL_XOUT_L,MPU6050_RA_ACCEL_XOUT_H, MPU6050_ADD); //reading raw accelerometer x axis
accel_y=I2CA_Read2Bytes(MPU6050_RA_ACCEL_YOUT_L,MPU6050_RA_ACCEL_YOUT_H, MPU6050_ADD);//reading raw accelerometer y axis
accel_z=I2CA_Read2Bytes(MPU6050_RA_ACCEL_ZOUT_L,MPU6050_RA_ACCEL_ZOUT_H, MPU6050_ADD);//reading raw accelerometer z axis
gyro_x=I2CA_Read2Bytes(MPU6050_RA_GYRO_XOUT_L,MPU6050_RA_GYRO_XOUT_H, MPU6050_ADD);//reading raw gyro x axis
gyro_y=I2CA_Read2Bytes(MPU6050_RA_GYRO_YOUT_L,MPU6050_RA_GYRO_YOUT_H, MPU6050_ADD);//reading raw gyro y axis
gyro_z=I2CA_Read2Bytes(MPU6050_RA_GYRO_ZOUT_L,MPU6050_RA_GYRO_ZOUT_H, MPU6050_ADD);//reading raw gyro z axis
ax=_IQdiv(_IQ((int)accel_x),_IQ(16384)); // converting raw data x to g
ay=_IQdiv(_IQ((int)accel_y),_IQ(16384)); // converting raw data y to g
az=_IQdiv(_IQ((int)accel_z),_IQ(16384)); // converting raw data z to g
gx=_IQdiv(_IQ((int)gyro_x),_IQ(65.5)); // converting raw gyro x to dps
gy=_IQdiv(_IQ((int)gyro_y),_IQ(65.5));// converting raw gyro y to dps
gz=_IQdiv(_IQ((int)gyro_z),_IQ(65.5));// converting raw gyro z to dps
roll = _IQatan2(ax,az);// roll angle
roll = _IQmpy(_IQ(180/3.14),roll); //converting to deg.
pitch = _IQatan2(ay,az);//_IQmpy(_IQ(180/3.14),_IQatan(1.0)); // pitch angle
pitch = _IQmpy(_IQ(180/3.14),pitch); // converting to deg
pitch_angle = _IQmpy(_IQ(0.96),(pitch_angle+_IQmpy(gy,_IQ(0.005))))+_IQmpy(_IQ(0.04),pitch); // data fusion gyro accelerometer
roll_angle = _IQmpy(_IQ(0.96),(roll_angle+_IQmpy(gx,_IQ(0.005))))+_IQmpy(_IQ(0.04),roll); // data fusion gyro acelerometer
cosRoll = _IQcos(_IQdiv(pitch_angle,_IQ(57.29582))); //cosine of roll angle calculations for tilt compensation in rad
sinRoll = _IQsin(_IQdiv(pitch_angle,_IQ(57.29582))); // sine of roll angle calculations for tilt compensation in rad
cosPitch = _IQcos(_IQdiv(roll_angle,_IQ(57.29582)));// cosine of pitch angle calculations for tilt compensation in rad
sinPitch = _IQsin(_IQdiv(roll_angle,_IQ(57.29582)));// sine of pitch angle calculations for tilt compensation in rad
mag_x=I2CA_Read2Bytes(HMC5883L_RA_DATAX_L,HMC5883L_RA_DATAX_H, HMC5883L_ADDRESS);
mag_y=I2CA_Read2Bytes(HMC5883L_RA_DATAY_L,HMC5883L_RA_DATAY_H, HMC5883L_ADDRESS);
mag_z=I2CA_Read2Bytes(HMC5883L_RA_DATAZ_L,HMC5883L_RA_DATAZ_H, HMC5883L_ADDRESS);
magx = _IQmpy(_IQ((int)mag_x),_IQ(0.73));
magy = _IQmpy(_IQ((int)mag_y),_IQ(0.73));
magz = _IQmpy(_IQ((int)mag_z),_IQ(0.73));
heading = _IQatan2(magy,magx);
Xh = _IQmpy(magx,cosPitch) + _IQmpy(magz,sinPitch);
Yh = _IQmpy(_IQmpy(magx,sinRoll), sinPitch) + _IQmpy(magy,cosRoll) - _IQmpy(_IQmpy(magz,sinRoll), cosPitch);
if(heading_tilt<0)
{
heading_tilt=heading_tilt+_IQ(6.28);
}
if (heading > _IQ(6.2918))
{
heading_tilt=heading_tilt-_IQ(6.2918);
}
heading_tilt = _IQmpy(heading_tilt,_IQ(57.29582));
GPIO_GPIO_setHigh(myGpio, GPIO_Number_0); // end of the code
}