# Good day everyone! :) Direction Cosine Matrix IMU need help~

Question asked by Sage on Mar 21, 2014
Latest reply on Mar 28, 2014 by Sage
Hello, I'm a newbie programmer here and is currently toying with my STM32F3_Discovery. My initial value of x, y and z of gyro printed are not 0, so I ventured on and found out about Kalman Filter and this, DCM. I wish to learn both, starting with the latter and so I arrived at some problems of which I do not yet comprehend. Hopefully someone here is willing to guide me of showing me some examples of this programming. When I tried to write matrix equation using array, it stripped off the gyro and other related functions (like magnetometer and accelerometer). And as I program into it next time and tried to print values, it gives me the old code of printing instead of the renewed one. I find this puzzling.. Also, I wish to learn how to program using rotation matrix instead of quaternion as I have not completely understood the latter yet. Thank you for noticing and your patience for reading through to the end. :D
Here is some part of my code:
#define ABS(x)         (x < 0) ? (-x) : x

#define L3G_Sensitivity_250dps     (float)   114.285f         /*!< gyroscope sensitivity with 250 dps full scale [LSB/dps] */
#define L3G_Sensitivity_500dps     (float)    57.1429f        /*!< gyroscope sensitivity with 500 dps full scale [LSB/dps] */
#define L3G_Sensitivity_2000dps    (float)    14.285f          /*!< gyroscope sensitivity with 2000 dps full scale [LSB/dps] */
#define PI                         (float)     3.14159265f

#define LSM_Acc_Sensitivity_2g     (float)     1.0f            /*!< accelerometer sensitivity with 2 g full scale [LSB/mg] */
#define LSM_Acc_Sensitivity_4g     (float)     0.5f            /*!< accelerometer sensitivity with 4 g full scale [LSB/mg] */
#define LSM_Acc_Sensitivity_8g     (float)     0.25f           /*!< accelerometer sensitivity with 8 g full scale [LSB/mg] */
#define LSM_Acc_Sensitivity_16g    (float)     0.0834f         /*!< accelerometer sensitivity with 12 g full scale [LSB/mg] */

/* Private variables ---------------------------------------------------------*/
RCC_ClocksTypeDef RCC_Clocks;
__IO uint32_t TimingDelay = 0;
__IO uint32_t UserButtonPressed = 0;
float MagBuffer[3] = {0.0f}, AccBuffer[3] = {0.0f}, Buffer[3] = {0.0f};
uint8_t Xval, Yval, Zval = 0x00;

__IO uint8_t PrevXferComplete = 1;

float fNormAcc,fSinRoll,fCosRoll,fSinPitch,fCosPitch = 0.0f, RollAng = 0.0f, PitchAng = 0.0f;
float fTiltedX,fTiltedY = 0.0f;
int R_Matrix[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; // R = Rotation Matrix
int Qp_Matrix[3][1] = {{0},{0},{0}};            // Qg = R Qp  Qg is vector ground, Qp is plane

/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/

uint32_t timerFlag=0;

/**
* @brief  Main program.
* @param  None
* @retval None
*/
int main(void)
{
uint8_t i = 0;
/* SysTick end of count event each 10ms */
RCC_GetClocksFreq(&RCC_Clocks);
SysTick_Config(RCC_Clocks.HCLK_Frequency / 100);

/* Initialize LEDs and User Button available on STM32F3-Discovery board */
STM_EVAL_LEDInit(LED3);
STM_EVAL_LEDInit(LED4);
STM_EVAL_LEDInit(LED5);
STM_EVAL_LEDInit(LED6);
STM_EVAL_LEDInit(LED7);
STM_EVAL_LEDInit(LED8);
STM_EVAL_LEDInit(LED9);
STM_EVAL_LEDInit(LED10);

STM_EVAL_PBInit(BUTTON_USER, BUTTON_MODE_EXTI);

/* Reset UserButton_Pressed variable */
UserButtonPressed = 0x00;

RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);

UARTConfig();
TIMConfig();
/* Infinite loop */
while (1)
{

/* Demo Gyroscope */
Demo_GyroConfig();

/* Read Gyro Angular data */

/* Update autoreload and capture compare registers value*/
Xval = ABS((int8_t)(Buffer[0]));
Yval = ABS((int8_t)(Buffer[1]));
Zval = ABS((int8_t)(Buffer[2]));
LEDOff();
if ( Xval>Yval)
{
if ((int8_t)Buffer[0] > 5.0f)
{
// LD10 On
STM_EVAL_LEDOn(LED10);
}
if ((int8_t)Buffer[0] < -5.0f)
{
// LD3 On
STM_EVAL_LEDOn(LED3);
}
}
else
{
if ((int8_t)Buffer[1] < -5.0f)
{
// LD6 on
STM_EVAL_LEDOn(LED6);
}
if ((int8_t)Buffer[1] > 5.0f)
{
// LD7 On
STM_EVAL_LEDOn(LED7);
}
}

sprintf(buf,"Xval=%d\t Yval=%d\t Zval=%d\r\n",Xval, Yval, Zval);
UART4_directprint(buf);
//Demo Compass
Demo_CompassConfig();

for(i=0;i<3;i++)
AccBuffer[i] /= 100.0f;

fNormAcc = sqrt((AccBuffer[0]*AccBuffer[0])+(AccBuffer[1]*AccBuffer[1])+(AccBuffer[2]*AccBuffer[2]));
fSinRoll = -AccBuffer[1]/fNormAcc;
fCosRoll = sqrt(1.0-(fSinRoll * fSinRoll));
fSinPitch = AccBuffer[0]/fNormAcc;
fCosPitch = sqrt(1.0-(fSinPitch * fSinPitch));
if ( fSinRoll >0)
{
if (fCosRoll>0)
{
RollAng = acos(fCosRoll)*180/PI;
}
else
{
RollAng = acos(fCosRoll)*180/PI + 180;
}
}
else
{
if (fCosRoll>0)
{
RollAng = acos(fCosRoll)*180/PI + 360;
}
else
{
RollAng = acos(fCosRoll)*180/PI + 180;
}
}

if ( fSinPitch >0)
{
if (fCosPitch>0)
{
PitchAng = acos(fCosPitch)*180/PI;
}
else
{
PitchAng = acos(fCosPitch)*180/PI + 180;
}
}
else
{
if (fCosPitch>0)
{
PitchAng = acos(fCosPitch)*180/PI + 360;
}
else
{
PitchAng = acos(fCosPitch)*180/PI + 180;
}
}

if (RollAng >=360)
{
RollAng = RollAng - 360;
}

if (PitchAng >=360)
{
PitchAng = PitchAng - 360;
}

fTiltedX = MagBuffer[0]*fCosPitch+MagBuffer[2]*fSinPitch;
fTiltedY = MagBuffer[0]*fSinRoll*fSinPitch+MagBuffer[1]*fCosRoll-MagBuffer[1]*fSinRoll*fCosPitch;

{
}

if ((RollAng <= 40.0f) && (PitchAng <= 40.0f))
{
{
STM_EVAL_LEDOn(LED10);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED6);
STM_EVAL_LEDOff(LED7);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED8);
STM_EVAL_LEDOff(LED9);
STM_EVAL_LEDOff(LED5);
}
{
STM_EVAL_LEDOn(LED9);
STM_EVAL_LEDOff(LED6);
STM_EVAL_LEDOff(LED10);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED8);
STM_EVAL_LEDOff(LED5);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED7);
}
{
STM_EVAL_LEDOn(LED7);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED9);
STM_EVAL_LEDOff(LED10);
STM_EVAL_LEDOff(LED8);
STM_EVAL_LEDOff(LED6);
STM_EVAL_LEDOff(LED5);
}
{
STM_EVAL_LEDOn(LED5);
STM_EVAL_LEDOff(LED6);
STM_EVAL_LEDOff(LED10);
STM_EVAL_LEDOff(LED8);
STM_EVAL_LEDOff(LED9);
STM_EVAL_LEDOff(LED7);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED3);
}
{
STM_EVAL_LEDOn(LED3);
STM_EVAL_LEDOff(LED6);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED8);
STM_EVAL_LEDOff(LED9);
STM_EVAL_LEDOff(LED5);
STM_EVAL_LEDOff(LED10);
STM_EVAL_LEDOff(LED7);
}
{
STM_EVAL_LEDOn(LED4);
STM_EVAL_LEDOff(LED6);
STM_EVAL_LEDOff(LED10);
STM_EVAL_LEDOff(LED8);
STM_EVAL_LEDOff(LED9);
STM_EVAL_LEDOff(LED5);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED7);
}
{
STM_EVAL_LEDOn(LED6);
STM_EVAL_LEDOff(LED9);
STM_EVAL_LEDOff(LED10);
STM_EVAL_LEDOff(LED8);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED5);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED7);
}
{
STM_EVAL_LEDOn(LED8);
STM_EVAL_LEDOff(LED6);
STM_EVAL_LEDOff(LED10);
STM_EVAL_LEDOff(LED7);
STM_EVAL_LEDOff(LED9);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED5);
}
}
else
{
LEDOff();
}
}
}