API tools faq
paste
canid

Navi

canid
Dec 10th, 2012
41
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 22.39 KB | None | 0 0
raw download clone embed print report
  1. #include "main.h"
  2.  
  3. /** @addtogroup STM32F3-Discovery_Demo
  4.   * @{
  5.   */
  6.  
  7. /* Private typedef -----------------------------------------------------------*/
  8. /* Private define ------------------------------------------------------------*/
  9. /* Private macro -------------------------------------------------------------*/
  10. #define ABS(x)         (x < 0) ? (-x) : x
  11.  
  12. #define L3G_Sensitivity_250dps     (float)   114.285f         /*!< gyroscope sensitivity with 250 dps full scale [LSB/dps] */
  13. #define L3G_Sensitivity_500dps     (float)    57.1429f        /*!< gyroscope sensitivity with 500 dps full scale [LSB/dps] */
  14. #define L3G_Sensitivity_2000dps    (float)    14.285f         /*!< gyroscope sensitivity with 2000 dps full scale [LSB/dps] */
  15. #define PI                         (float)     3.14159265f
  16.  
  17. #define LSM_Acc_Sensitivity_2g     (float)     1.0f            /*!< accelerometer sensitivity with 2 g full scale [LSB/mg] */
  18. #define LSM_Acc_Sensitivity_4g     (float)     0.5f            /*!< accelerometer sensitivity with 4 g full scale [LSB/mg] */
  19. #define LSM_Acc_Sensitivity_8g     (float)     0.25f           /*!< accelerometer sensitivity with 8 g full scale [LSB/mg] */
  20. #define LSM_Acc_Sensitivity_16g    (float)     0.0834f         /*!< accelerometer sensitivity with 12 g full scale [LSB/mg] */
  21.  
  22. /* Private variables ---------------------------------------------------------*/
  23.   RCC_ClocksTypeDef RCC_Clocks;
  24. __IO uint32_t TimingDelay = 0;
  25. __IO uint32_t UserButtonPressed = 0;
  26. __IO float HeadingValue = 0.0f;  
  27. float MagBuffer[3] = {0.0f}, AccBuffer[3] = {0.0f}, Buffer[3] = {0.0f};
  28. uint8_t Xval, Yval = 0x00;
  29.  
  30. __IO uint8_t DataReady = 0;
  31. __IO uint8_t PrevXferComplete = 1;
  32. __IO uint32_t USBConnectTimeOut = 100;
  33.  
  34. float fNormAcc,fSinRoll,fCosRoll,fSinPitch,fCosPitch = 0.0f, RollAng = 0.0f, PitchAng = 0.0f;
  35. float fSinYaw,fCosYaw= 0.0f, YawAng = 0.0f;
  36. float fTiltedX,fTiltedY = 0.0f;
  37. /* Private function prototypes -----------------------------------------------*/
  38. /* Private functions ---------------------------------------------------------*/
  39.  
  40. /**
  41.   * @brief  Main program.
  42.   * @param  None
  43.   * @retval None
  44.   */
  45. int main(void)
  46. {  
  47.   uint8_t i = 0;
  48.   /* SysTick end of count event each 10ms */
  49.   RCC_GetClocksFreq(&RCC_Clocks);
  50.   SysTick_Config(RCC_Clocks.HCLK_Frequency / 100);
  51.  
  52.   /* Initialize LEDs and User Button available on STM32F3-Discovery board */
  53.   STM_EVAL_LEDToggle(LED3);
  54.   STM_EVAL_LEDInit(LED4);
  55.   STM_EVAL_LEDInit(LED5);
  56.   STM_EVAL_LEDInit(LED6);
  57.   STM_EVAL_LEDInit(LED7);
  58.   STM_EVAL_LEDInit(LED8);
  59.   STM_EVAL_LEDInit(LED9);
  60.   STM_EVAL_LEDInit(LED10);
  61.  
  62.   STM_EVAL_PBInit(BUTTON_USER, BUTTON_MODE_EXTI);
  63.  
  64.   /* Configure the USB */
  65.   Demo_USB();
  66.  
  67.   /* Reset UserButton_Pressed variable */
  68.   UserButtonPressed = 0x00;
  69.    
  70.   /* Main loop */
  71.   while (1)
  72.   {  
  73.     /* Waiting User Button is pressed */
  74.     while (UserButtonPressed == 0x00)
  75.     {
  76.       // Lights out.
  77.       STM_EVAL_LEDOff(LED3);
  78.       STM_EVAL_LEDOff(LED4);
  79.       STM_EVAL_LEDOff(LED5);
  80.       STM_EVAL_LEDOff(LED6);
  81.       STM_EVAL_LEDOff(LED7);
  82.       STM_EVAL_LEDOff(LED8);
  83.       STM_EVAL_LEDOff(LED9);
  84.       STM_EVAL_LEDOff(LED10);
  85.       __WFE();
  86.     }
  87.    
  88.     DataReady = 0x00;
  89.    
  90.     /* Demo Compass */
  91.     Demo_CompassConfig();
  92.    
  93.     /* Waiting User Button is pressed */
  94.     while (UserButtonPressed == 0x01)
  95.     {
  96.       /* Wait for data ready */
  97.       while(DataReady !=0x05)
  98.       {}
  99.       DataReady = 0x00;
  100.      
  101.       /* Read Compass data */
  102.       Demo_CompassReadMag(MagBuffer);
  103.       Demo_CompassReadAcc(AccBuffer);
  104.      
  105.       for(i=0;i<3;i++)
  106.         AccBuffer[i] /= 100.0f;
  107.      
  108.       fNormAcc = sqrt((AccBuffer[0]*AccBuffer[0])+(AccBuffer[1]*AccBuffer[1])+(AccBuffer[2]*AccBuffer[2]));
  109.      
  110.       fSinRoll = -AccBuffer[1]/fNormAcc;
  111.       fCosRoll = sqrt(1.0-(fSinRoll * fSinRoll));
  112.       fSinPitch = AccBuffer[0]/fNormAcc;
  113.       fCosPitch = sqrt(1.0-(fSinPitch * fSinPitch));
  114.      if ( fSinRoll >0)
  115.      {
  116.        if (fCosRoll>0)
  117.        {
  118.          RollAng = acos(fCosRoll)*180/PI;
  119.        }
  120.        else
  121.        {
  122.          RollAng = acos(fCosRoll)*180/PI + 180;
  123.        }
  124.      }
  125.      else
  126.      {
  127.        if (fCosRoll>0)
  128.        {
  129.          RollAng = acos(fCosRoll)*180/PI + 360;
  130.        }
  131.        else
  132.        {
  133.          RollAng = acos(fCosRoll)*180/PI + 180;
  134.        }
  135.      }
  136.      
  137.       if ( fSinPitch >0)
  138.      {
  139.        if (fCosPitch>0)
  140.        {
  141.             PitchAng = acos(fCosPitch)*180/PI;
  142.        }
  143.        else
  144.        {
  145.           PitchAng = acos(fCosPitch)*180/PI + 180;
  146.        }
  147.      }
  148.      else
  149.      {
  150.        if (fCosPitch>0)
  151.        {
  152.             PitchAng = acos(fCosPitch)*180/PI + 360;
  153.        }
  154.        else
  155.        {
  156.           PitchAng = acos(fCosPitch)*180/PI + 180;
  157.        }
  158.      }
  159.  
  160.       if (RollAng >=360)
  161.       {
  162.         RollAng = RollAng - 360;
  163.       }
  164.      
  165.       if (PitchAng >=360)
  166.       {
  167.         PitchAng = PitchAng - 360;
  168.       }
  169.      
  170.       fTiltedX = MagBuffer[0]*fCosPitch+MagBuffer[2]*fSinPitch;
  171.       fTiltedY = MagBuffer[0]*fSinRoll*fSinPitch+MagBuffer[1]*fCosRoll-MagBuffer[1]*fSinRoll*fCosPitch;
  172.      
  173.       HeadingValue = (float) ((atan2f((float)fTiltedY,(float)fTiltedX))*180)/PI;
  174.  
  175.       if (HeadingValue < 0)
  176.       {
  177.         HeadingValue = HeadingValue + 360;    
  178.       }
  179.      
  180.       if ((RollAng <= 40.0f) && (PitchAng <= 40.0f))
  181.       {
  182.           if (((HeadingValue < 12.25f)&&(HeadingValue >= 0.0f))||((HeadingValue >=349.75f)&&(HeadingValue <= 360.0f))) // S *
  183.           {
  184.             STM_EVAL_LEDOn(LED10);
  185.             STM_EVAL_LEDOff(LED3);
  186.             STM_EVAL_LEDOff(LED6);
  187.             STM_EVAL_LEDOff(LED7);
  188.             STM_EVAL_LEDOff(LED4);
  189.             STM_EVAL_LEDOff(LED8);
  190.             STM_EVAL_LEDOff(LED9);
  191.             STM_EVAL_LEDOff(LED5);
  192.           }
  193.         else if ((HeadingValue <34.75f)&&(HeadingValue >=12.25f)) // SSE
  194.         {
  195.             STM_EVAL_LEDOff(LED8);
  196.             STM_EVAL_LEDOff(LED6);
  197.             STM_EVAL_LEDOn(LED10);
  198.             STM_EVAL_LEDOff(LED7);
  199.             STM_EVAL_LEDOn(LED9);
  200.             STM_EVAL_LEDOff(LED3);
  201.             STM_EVAL_LEDOff(LED4);
  202.             STM_EVAL_LEDOff(LED5);
  203.         }
  204.           else  if ((HeadingValue <57.25f)&&(HeadingValue >= 34.75f)) // SE
  205.           {
  206.             STM_EVAL_LEDOn(LED9);
  207.             STM_EVAL_LEDOff(LED6);
  208.             STM_EVAL_LEDOff(LED10);
  209.             STM_EVAL_LEDOff(LED3);
  210.             STM_EVAL_LEDOff(LED8);
  211.             STM_EVAL_LEDOff(LED5);
  212.             STM_EVAL_LEDOff(LED4);
  213.             STM_EVAL_LEDOff(LED7);
  214.           }
  215.         else if ((HeadingValue <79.75f)&&(HeadingValue >=57.25f)) // ESE
  216.         {
  217.             STM_EVAL_LEDOff(LED8);
  218.             STM_EVAL_LEDOff(LED6);
  219.             STM_EVAL_LEDOff(LED10);
  220.             STM_EVAL_LEDOn(LED7);
  221.             STM_EVAL_LEDOn(LED9);
  222.             STM_EVAL_LEDOff(LED3);
  223.             STM_EVAL_LEDOff(LED4);
  224.             STM_EVAL_LEDOff(LED5);
  225.         }
  226.           else  if ((HeadingValue < 102.25f)&&(HeadingValue >= 79.75f)) // E
  227.           {
  228.             STM_EVAL_LEDOn(LED7);
  229.             STM_EVAL_LEDOff(LED3);
  230.             STM_EVAL_LEDOff(LED4);
  231.             STM_EVAL_LEDOff(LED9);
  232.             STM_EVAL_LEDOff(LED10);
  233.             STM_EVAL_LEDOff(LED8);
  234.             STM_EVAL_LEDOff(LED6);
  235.             STM_EVAL_LEDOff(LED5);
  236.           }
  237.         else if ((HeadingValue <124.75f)&&(HeadingValue >=102.25f)) // ENE
  238.         {
  239.             STM_EVAL_LEDOff(LED8);
  240.             STM_EVAL_LEDOff(LED6);
  241.             STM_EVAL_LEDOff(LED10);
  242.             STM_EVAL_LEDOn(LED7);
  243.             STM_EVAL_LEDOff(LED9);
  244.             STM_EVAL_LEDOff(LED3);
  245.             STM_EVAL_LEDOff(LED4);
  246.             STM_EVAL_LEDOn(LED5);
  247.         }
  248.           else  if ((HeadingValue <147.25f)&&(HeadingValue >= 124.75f)) // NE
  249.           {
  250.             STM_EVAL_LEDOn(LED5);
  251.             STM_EVAL_LEDOff(LED6);
  252.             STM_EVAL_LEDOff(LED10);
  253.             STM_EVAL_LEDOff(LED8);
  254.             STM_EVAL_LEDOff(LED9);
  255.             STM_EVAL_LEDOff(LED7);
  256.             STM_EVAL_LEDOff(LED4);
  257.             STM_EVAL_LEDOff(LED3);
  258.           }
  259.         else if ((HeadingValue <169.75f)&&(HeadingValue >=147.25f)) // NNE
  260.         {
  261.             STM_EVAL_LEDOff(LED8);
  262.             STM_EVAL_LEDOff(LED6);
  263.             STM_EVAL_LEDOff(LED10);
  264.             STM_EVAL_LEDOff(LED7);
  265.             STM_EVAL_LEDOff(LED9);
  266.             STM_EVAL_LEDOn(LED3);
  267.             STM_EVAL_LEDOff(LED4);
  268.             STM_EVAL_LEDOn(LED5);
  269.         }
  270.           else  if ((HeadingValue <192.25f)&&(HeadingValue >= 169.75f)) // N
  271.           {
  272.             STM_EVAL_LEDOn(LED3);
  273.             STM_EVAL_LEDOff(LED6);
  274.             STM_EVAL_LEDOff(LED4);
  275.             STM_EVAL_LEDOff(LED8);
  276.             STM_EVAL_LEDOff(LED9);
  277.             STM_EVAL_LEDOff(LED5);
  278.             STM_EVAL_LEDOff(LED10);
  279.             STM_EVAL_LEDOff(LED7);
  280.           }
  281.         else if ((HeadingValue <214.75f)&&(HeadingValue >=192.25f)) // NNW
  282.         {
  283.             STM_EVAL_LEDOff(LED8);
  284.             STM_EVAL_LEDOff(LED6);
  285.             STM_EVAL_LEDOff(LED10);
  286.             STM_EVAL_LEDOff(LED7);
  287.             STM_EVAL_LEDOff(LED9);
  288.             STM_EVAL_LEDOn(LED3);
  289.             STM_EVAL_LEDOn(LED4);
  290.             STM_EVAL_LEDOff(LED5);
  291.         }
  292.           else  if ((HeadingValue <237.25f)&&(HeadingValue >= 214.75f)) // NW
  293.           {
  294.             STM_EVAL_LEDOn(LED4);
  295.             STM_EVAL_LEDOff(LED6);
  296.             STM_EVAL_LEDOff(LED10);
  297.             STM_EVAL_LEDOff(LED8);
  298.             STM_EVAL_LEDOff(LED9);
  299.             STM_EVAL_LEDOff(LED5);
  300.             STM_EVAL_LEDOff(LED3);
  301.             STM_EVAL_LEDOff(LED7);
  302.           }
  303.         else if ((HeadingValue <259.75f)&&(HeadingValue >=237.25f)) // WNW
  304.         {
  305.             STM_EVAL_LEDOff(LED8);
  306.             STM_EVAL_LEDOn(LED6);
  307.             STM_EVAL_LEDOff(LED10);
  308.             STM_EVAL_LEDOff(LED7);
  309.             STM_EVAL_LEDOff(LED9);
  310.             STM_EVAL_LEDOff(LED3);
  311.             STM_EVAL_LEDOn(LED4);
  312.             STM_EVAL_LEDOff(LED5);
  313.         }
  314.           else  if ((HeadingValue < 282.25f)&&(HeadingValue >= 259.75f)) // W
  315.           {
  316.             STM_EVAL_LEDOn(LED6);
  317.             STM_EVAL_LEDOff(LED9);
  318.             STM_EVAL_LEDOff(LED10);
  319.             STM_EVAL_LEDOff(LED8);
  320.             STM_EVAL_LEDOff(LED3);
  321.             STM_EVAL_LEDOff(LED5);
  322.             STM_EVAL_LEDOff(LED4);
  323.             STM_EVAL_LEDOff(LED7);
  324.           }
  325.         else if ((HeadingValue <304.75f)&&(HeadingValue >=282.25f)) // WSE
  326.         {
  327.             STM_EVAL_LEDOn(LED8);
  328.             STM_EVAL_LEDOn(LED6);
  329.             STM_EVAL_LEDOff(LED10);
  330.             STM_EVAL_LEDOff(LED7);
  331.             STM_EVAL_LEDOff(LED9);
  332.             STM_EVAL_LEDOff(LED3);
  333.             STM_EVAL_LEDOff(LED4);
  334.             STM_EVAL_LEDOff(LED5);
  335.         }
  336.           else  if ((HeadingValue < 327.25f)&&(HeadingValue >= 304.75f)) // SW
  337.           {
  338.             STM_EVAL_LEDOn(LED8);
  339.             STM_EVAL_LEDOff(LED6);
  340.             STM_EVAL_LEDOff(LED10);
  341.             STM_EVAL_LEDOff(LED7);
  342.             STM_EVAL_LEDOff(LED9);
  343.             STM_EVAL_LEDOff(LED3);
  344.             STM_EVAL_LEDOff(LED4);
  345.             STM_EVAL_LEDOff(LED5);
  346.           }
  347.         else if ((HeadingValue <349.75f)&&(HeadingValue >=327.25f)) // SSW
  348.         {
  349.             STM_EVAL_LEDOn(LED8);
  350.             STM_EVAL_LEDOff(LED6);
  351.             STM_EVAL_LEDOn(LED10);
  352.             STM_EVAL_LEDOff(LED7);
  353.             STM_EVAL_LEDOff(LED9);
  354.             STM_EVAL_LEDOff(LED3);
  355.             STM_EVAL_LEDOff(LED4);
  356.             STM_EVAL_LEDOff(LED5);
  357.         }
  358.       } else {
  359.           STM_EVAL_LEDOn(LED3);
  360.           STM_EVAL_LEDOn(LED4);
  361.           STM_EVAL_LEDOn(LED5);
  362.           STM_EVAL_LEDOn(LED6);
  363.           STM_EVAL_LEDOn(LED7);
  364.           STM_EVAL_LEDOn(LED8);
  365.           STM_EVAL_LEDOn(LED9);
  366.           STM_EVAL_LEDOn(LED10);
  367.       }
  368.     }
  369.  
  370.     while (UserButtonPressed == 0x02) // Clinometer.
  371.     {
  372.         Demo_CompassReadAcc(AccBuffer);
  373.         Demo_GyroReadAngRate(Buffer);
  374.  
  375.         // Commence fall over and die here:
  376.         AccBuffer[2] /= 100.0f;
  377.  
  378.         fNormAcc = sqrt((AccBuffer[0]*AccBuffer[0])+(AccBuffer[1]*AccBuffer[1])+(AccBuffer[2]*AccBuffer[2]));
  379.         fSinYaw = AccBuffer[2]/fNormAcc;
  380.         fCosYaw = sqrt(1.0-(fSinYaw * fSinYaw));
  381.        if ( fSinYaw >0)
  382.        {
  383.          if (fCosYaw>0)
  384.          {
  385.            YawAng = acos(fCosYaw)*180/PI;
  386.          }
  387.          else
  388.          {
  389.            YawAng = acos(fCosYaw)*180/PI + 180;
  390.          }
  391.        }
  392.        else
  393.        {
  394.          if (fCosYaw>0)
  395.          {
  396.            YawAng = acos(fCosYaw)*180/PI + 360;
  397.          }
  398.          else
  399.          {
  400.            YawAng = acos(fCosYaw)*180/PI + 180;
  401.          }
  402.        }
  403.         if (YawAng >=360)
  404.         {
  405.           YawAng = YawAng - 360;
  406.         }
  407.     }
  408.  
  409.   }
  410. }
  411.  
  412.  
  413. /**
  414.   * @brief  Configure the USB.
  415.   * @param  None
  416.   * @retval None
  417.   */
  418. void Demo_USB (void)
  419. {
  420.   Set_System();
  421.   Set_USBClock();
  422.   USB_Interrupts_Config();
  423.  
  424.   USB_Init();
  425.  
  426.   while ((bDeviceState != CONFIGURED)&&(USBConnectTimeOut != 0))
  427.   {}
  428. }
  429.  
  430. /**
  431.   * @brief  Configure the Mems to gyroscope application.
  432.   * @param  None
  433.   * @retval None
  434.   */
  435. void Demo_GyroConfig(void)
  436. {
  437.   L3GD20_InitTypeDef L3GD20_InitStructure;
  438.   L3GD20_FilterConfigTypeDef L3GD20_FilterStructure;
  439.  
  440.   /* Configure Mems L3GD20 */
  441.   L3GD20_InitStructure.Power_Mode = L3GD20_MODE_ACTIVE;
  442.   L3GD20_InitStructure.Output_DataRate = L3GD20_OUTPUT_DATARATE_1;
  443.   L3GD20_InitStructure.Axes_Enable = L3GD20_AXES_ENABLE;
  444.   L3GD20_InitStructure.Band_Width = L3GD20_BANDWIDTH_4;
  445.   L3GD20_InitStructure.BlockData_Update = L3GD20_BlockDataUpdate_Continous;
  446.   L3GD20_InitStructure.Endianness = L3GD20_BLE_LSB;
  447.   L3GD20_InitStructure.Full_Scale = L3GD20_FULLSCALE_500;
  448.   L3GD20_Init(&L3GD20_InitStructure);
  449.    
  450.   L3GD20_FilterStructure.HighPassFilter_Mode_Selection =L3GD20_HPM_NORMAL_MODE_RES;
  451.   L3GD20_FilterStructure.HighPassFilter_CutOff_Frequency = L3GD20_HPFCF_0;
  452.   L3GD20_FilterConfig(&L3GD20_FilterStructure) ;
  453.  
  454.   L3GD20_FilterCmd(L3GD20_HIGHPASSFILTER_ENABLE);
  455. }
  456.  
  457. /**
  458.   * @brief  Calculate the angular Data rate Gyroscope.
  459.   * @param  pfData : Data out pointer
  460.   * @retval None
  461.   */
  462. void Demo_GyroReadAngRate (float* pfData)
  463. {
  464.   uint8_t tmpbuffer[6] ={0};
  465.   int16_t RawData[3] = {0};
  466.   uint8_t tmpreg = 0;
  467.   float sensitivity = 0;
  468.   int i =0;
  469.  
  470.   L3GD20_Read(&tmpreg,L3GD20_CTRL_REG4_ADDR,1);
  471.  
  472.   L3GD20_Read(tmpbuffer,L3GD20_OUT_X_L_ADDR,6);
  473.  
  474.   /* check in the control register 4 the data alignment (Big Endian or Little Endian)*/
  475.   if(!(tmpreg & 0x40))
  476.   {
  477.     for(i=0; i<3; i++)
  478.     {
  479.       RawData[i]=(int16_t)(((uint16_t)tmpbuffer[2*i+1] << 8) + tmpbuffer[2*i]);
  480.     }
  481.   }
  482.   else
  483.   {
  484.     for(i=0; i<3; i++)
  485.     {
  486.       RawData[i]=(int16_t)(((uint16_t)tmpbuffer[2*i] << 8) + tmpbuffer[2*i+1]);
  487.     }
  488.   }
  489.  
  490.   /* Switch the sensitivity value set in the CRTL4 */
  491.   switch(tmpreg & 0x30)
  492.   {
  493.   case 0x00:
  494.     sensitivity=L3G_Sensitivity_250dps;
  495.     break;
  496.    
  497.   case 0x10:
  498.     sensitivity=L3G_Sensitivity_500dps;
  499.     break;
  500.    
  501.   case 0x20:
  502.     sensitivity=L3G_Sensitivity_2000dps;
  503.     break;
  504.   }
  505.   /* divide by sensitivity */
  506.   for(i=0; i<3; i++)
  507.   {
  508.     pfData[i]=(float)RawData[i]/sensitivity;
  509.   }
  510. }
  511.  
  512. /**
  513.   * @brief  Configure the Mems to compass application.
  514.   * @param  None
  515.   * @retval None
  516.   */
  517. void Demo_CompassConfig(void)
  518. {
  519.   LSM303DLHCMag_InitTypeDef LSM303DLHC_InitStructure;
  520.   LSM303DLHCAcc_InitTypeDef LSM303DLHCAcc_InitStructure;
  521.   LSM303DLHCAcc_FilterConfigTypeDef LSM303DLHCFilter_InitStructure;
  522.  
  523.   /* Configure MEMS magnetometer main parameters: temp, working mode, full Scale and Data rate */
  524.   LSM303DLHC_InitStructure.Temperature_Sensor = LSM303DLHC_TEMPSENSOR_DISABLE;
  525.   LSM303DLHC_InitStructure.MagOutput_DataRate =LSM303DLHC_ODR_30_HZ ;
  526.   LSM303DLHC_InitStructure.MagFull_Scale = LSM303DLHC_FS_8_1_GA;
  527.   LSM303DLHC_InitStructure.Working_Mode = LSM303DLHC_CONTINUOS_CONVERSION;
  528.   LSM303DLHC_MagInit(&LSM303DLHC_InitStructure);
  529.  
  530.    /* Fill the accelerometer structure */
  531.   LSM303DLHCAcc_InitStructure.Power_Mode = LSM303DLHC_NORMAL_MODE;
  532.   LSM303DLHCAcc_InitStructure.AccOutput_DataRate = LSM303DLHC_ODR_50_HZ;
  533.   LSM303DLHCAcc_InitStructure.Axes_Enable= LSM303DLHC_AXES_ENABLE;
  534.   LSM303DLHCAcc_InitStructure.AccFull_Scale = LSM303DLHC_FULLSCALE_2G;
  535.   LSM303DLHCAcc_InitStructure.BlockData_Update = LSM303DLHC_BlockUpdate_Continous;
  536.   LSM303DLHCAcc_InitStructure.Endianness=LSM303DLHC_BLE_LSB;
  537.   LSM303DLHCAcc_InitStructure.High_Resolution=LSM303DLHC_HR_ENABLE;
  538.   /* Configure the accelerometer main parameters */
  539.   LSM303DLHC_AccInit(&LSM303DLHCAcc_InitStructure);
  540.  
  541.   /* Fill the accelerometer LPF structure */
  542.   LSM303DLHCFilter_InitStructure.HighPassFilter_Mode_Selection =LSM303DLHC_HPM_NORMAL_MODE;
  543.   LSM303DLHCFilter_InitStructure.HighPassFilter_CutOff_Frequency = LSM303DLHC_HPFCF_16;
  544.   LSM303DLHCFilter_InitStructure.HighPassFilter_AOI1 = LSM303DLHC_HPF_AOI1_DISABLE;
  545.   LSM303DLHCFilter_InitStructure.HighPassFilter_AOI2 = LSM303DLHC_HPF_AOI2_DISABLE;
  546.  
  547.   /* Configure the accelerometer LPF main parameters */
  548.   LSM303DLHC_AccFilterConfig(&LSM303DLHCFilter_InitStructure);
  549. }
  550.  
  551. /**
  552. * @brief Read LSM303DLHC output register, and calculate the acceleration ACC=(1/SENSITIVITY)* (out_h*256+out_l)/16 (12 bit rappresentation)
  553. * @param pnData: pointer to float buffer where to store data
  554. * @retval None
  555. */
  556. void Demo_CompassReadAcc(float* pfData)
  557. {
  558.   int16_t pnRawData[3];
  559.   uint8_t ctrlx[2];
  560.   uint8_t buffer[6], cDivider;
  561.   uint8_t i = 0;
  562.   float LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_2g;
  563.  
  564.   /* Read the register content */
  565.   LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG4_A, ctrlx,2);
  566.   LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_X_L_A, buffer, 6);
  567.    
  568.   if(ctrlx[1]&0x40)
  569.     cDivider=64;
  570.   else
  571.     cDivider=16;
  572.  
  573.   /* check in the control register4 the data alignment*/
  574.   if(!(ctrlx[0] & 0x40) || (ctrlx[1] & 0x40)) /* Little Endian Mode or FIFO mode */
  575.   {
  576.     for(i=0; i<3; i++)
  577.     {
  578.       pnRawData[i]=((int16_t)((uint16_t)buffer[2*i+1] << 8) + buffer[2*i])/cDivider;
  579.     }
  580.   }
  581.   else /* Big Endian Mode */
  582.   {
  583.     for(i=0; i<3; i++)
  584.       pnRawData[i]=((int16_t)((uint16_t)buffer[2*i] << 8) + buffer[2*i+1])/cDivider;
  585.   }
  586.   /* Read the register content */
  587.   LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG4_A, ctrlx,2);
  588.  
  589.  
  590.   if(ctrlx[1]&0x40)
  591.   {
  592.     /* FIFO mode */
  593.     LSM_Acc_Sensitivity = 0.25;
  594.   }
  595.   else
  596.   {
  597.     /* normal mode */
  598.     /* switch the sensitivity value set in the CRTL4*/
  599.     switch(ctrlx[0] & 0x30)
  600.     {
  601.     case LSM303DLHC_FULLSCALE_2G:
  602.       LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_2g;
  603.       break;
  604.     case LSM303DLHC_FULLSCALE_4G:
  605.       LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_4g;
  606.       break;
  607.     case LSM303DLHC_FULLSCALE_8G:
  608.       LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_8g;
  609.       break;
  610.     case LSM303DLHC_FULLSCALE_16G:
  611.       LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_16g;
  612.       break;
  613.     }
  614.   }
  615.  
  616.   /* Obtain the mg value for the three axis */
  617.   for(i=0; i<3; i++)
  618.   {
  619.     pfData[i]=(float)pnRawData[i]/LSM_Acc_Sensitivity;
  620.   }
  621.  
  622. }
  623.  
  624. /**
  625.   * @brief  calculate the magnetic field Magn.
  626. * @param  pfData: pointer to the data out
  627.   * @retval None
  628.   */
  629. void Demo_CompassReadMag (float* pfData)
  630. {
  631.   static uint8_t buffer[6] = {0};
  632.   uint8_t CTRLB = 0;
  633.   uint16_t Magn_Sensitivity_XY = 0, Magn_Sensitivity_Z = 0;
  634.   uint8_t i =0;
  635.   LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_CRB_REG_M, &CTRLB, 1);
  636.  
  637.   LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_H_M, buffer, 1);
  638.   LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_L_M, buffer+1, 1);
  639.   LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_H_M, buffer+2, 1);
  640.   LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_L_M, buffer+3, 1);
  641.   LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_H_M, buffer+4, 1);
  642.   LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_L_M, buffer+5, 1);
  643.   /* Switch the sensitivity set in the CRTLB*/
  644.   switch(CTRLB & 0xE0)
  645.   {
  646.   case LSM303DLHC_FS_1_3_GA:
  647.     Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_1_3Ga;
  648.     Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_1_3Ga;
  649.     break;
  650.   case LSM303DLHC_FS_1_9_GA:
  651.     Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_1_9Ga;
  652.     Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_1_9Ga;
  653.     break;
  654.   case LSM303DLHC_FS_2_5_GA:
  655.     Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_2_5Ga;
  656.     Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_2_5Ga;
  657.     break;
  658.   case LSM303DLHC_FS_4_0_GA:
  659.     Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_4Ga;
  660.     Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_4Ga;
  661.     break;
  662.   case LSM303DLHC_FS_4_7_GA:
  663.     Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_4_7Ga;
  664.     Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_4_7Ga;
  665.     break;
  666.   case LSM303DLHC_FS_5_6_GA:
  667.     Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_5_6Ga;
  668.     Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_5_6Ga;
  669.     break;
  670.   case LSM303DLHC_FS_8_1_GA:
  671.     Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_8_1Ga;
  672.     Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_8_1Ga;
  673.     break;
  674.   }
  675.  
  676.   for(i=0; i<2; i++)
  677.   {
  678.     pfData[i]=(float)((int16_t)(((uint16_t)buffer[2*i] << 8) + buffer[2*i+1])*1000)/Magn_Sensitivity_XY;
  679.   }
  680.   pfData[2]=(float)((int16_t)(((uint16_t)buffer[4] << 8) + buffer[5])*1000)/Magn_Sensitivity_Z;
  681. }
  682.  
  683. /**
  684.   * @brief  Inserts a delay time.
  685.   * @param  nTime: specifies the delay time length, in 10 ms.
  686.   * @retval None
  687.   */
  688. void Delay(__IO uint32_t nTime)
  689. {
  690.   TimingDelay = nTime;
  691.  
  692.   while(TimingDelay != 0);
  693. }
  694.  
  695. /**
  696.   * @brief  Decrements the TimingDelay variable.
  697.   * @param  None
  698.   * @retval None
  699.   */
  700. void TimingDelay_Decrement(void)
  701. {
  702.   if (TimingDelay != 0x00)
  703.   {
  704.     TimingDelay--;
  705.   }
  706. }
  707.  
  708. /**
  709.   * @brief  Basic management of the timeout situation.
  710.   * @param  None.
  711.   * @retval None.
  712.   */
  713. uint32_t LSM303DLHC_TIMEOUT_UserCallback(void)
  714. {
  715.   return 0;
  716. }
  717.  
  718. /**
  719.   * @brief  Basic management of the timeout situation.
  720.   * @param  None.
  721.   * @retval None.
  722.   */
  723. uint32_t L3GD20_TIMEOUT_UserCallback(void)
  724. {
  725.   return 0;
  726. }
  727.  
  728. #ifdef  USE_FULL_ASSERT
  729.  
  730. /**
  731.   * @brief  Reports the name of the source file and the source line number
  732.   *         where the assert_param error has occurred.
  733.   * @param  file: pointer to the source file name
  734.   * @param  line: assert_param error line source number
  735.   * @retval None
  736.   */
  737. void assert_failed(uint8_t* file, uint32_t line)
  738. {
  739.   /* User can add his own implementation to report the file name and line number,
  740.      ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  741.  
  742.   /* Infinite loop */
  743.   while (1)
  744.   {
  745.   }
  746. }
  747. #endif
  748.  
  749. /**
  750.   * @}
  751.   */
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!