Diff: compass.c

Revision:

3:93e488fbb8a2

Parent:

1:1b37c4b989b4

--- a/compass.c	Tue May 17 23:53:10 2016 +0000
+++ b/compass.c	Wed May 16 02:39:14 2018 +0000
@@ -3,114 +3,91 @@
 #include <stdio.h>
 #include "stm32f3_discovery_lsm303dlhc.h"
 
-#define PI                         (float)     3.14159265f
+#define GPIO_MODE_INPUT 0
+#define GPIO_MODE_OUTPUT 1
+#define GPIO_MODE_AF 2
 
-#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] */
-#define ABS(x)         (x < 0) ? (-x) : x
+#define GPIO_NO_PULL 0
+#define GPIO_PULL_UP 1
+#define GPIO_PULL_DOWN 2
 
-void Demo_CompassConfig(void)
-{
-  LSM303DLHCMag_InitTypeDef LSM303DLHC_InitStructure;
-  LSM303DLHCAcc_InitTypeDef LSM303DLHCAcc_InitStructure;
-  LSM303DLHCAcc_FilterConfigTypeDef LSM303DLHCFilter_InitStructure;
-  
-  /* Configure MEMS magnetometer main parameters: temp, working mode, full Scale and Data rate */
-  LSM303DLHC_InitStructure.Temperature_Sensor = LSM303DLHC_TEMPSENSOR_DISABLE;
-  LSM303DLHC_InitStructure.MagOutput_DataRate =LSM303DLHC_ODR_30_HZ ;
-  LSM303DLHC_InitStructure.MagFull_Scale = LSM303DLHC_FS_8_1_GA;
-  LSM303DLHC_InitStructure.Working_Mode = LSM303DLHC_CONTINUOS_CONVERSION;
-  LSM303DLHC_MagInit(&LSM303DLHC_InitStructure);
-  
-   /* Fill the accelerometer structure */
-  LSM303DLHCAcc_InitStructure.Power_Mode = LSM303DLHC_NORMAL_MODE;
-  LSM303DLHCAcc_InitStructure.AccOutput_DataRate = LSM303DLHC_ODR_50_HZ;
-  LSM303DLHCAcc_InitStructure.Axes_Enable= LSM303DLHC_AXES_ENABLE;
-  LSM303DLHCAcc_InitStructure.AccFull_Scale = LSM303DLHC_FULLSCALE_2G;
-  LSM303DLHCAcc_InitStructure.BlockData_Update = LSM303DLHC_BlockUpdate_Continous;
-  LSM303DLHCAcc_InitStructure.Endianness=LSM303DLHC_BLE_LSB;
-  LSM303DLHCAcc_InitStructure.High_Resolution=LSM303DLHC_HR_ENABLE;
-  /* Configure the accelerometer main parameters */
-  LSM303DLHC_AccInit(&LSM303DLHCAcc_InitStructure);
-  
-  /* Fill the accelerometer LPF structure */
-  LSM303DLHCFilter_InitStructure.HighPassFilter_Mode_Selection =LSM303DLHC_HPM_NORMAL_MODE;
-  LSM303DLHCFilter_InitStructure.HighPassFilter_CutOff_Frequency = LSM303DLHC_HPFCF_16;
-  LSM303DLHCFilter_InitStructure.HighPassFilter_AOI1 = LSM303DLHC_HPF_AOI1_DISABLE;
-  LSM303DLHCFilter_InitStructure.HighPassFilter_AOI2 = LSM303DLHC_HPF_AOI2_DISABLE;
+#define GPIO_OUTPUT_PUSH_PULL 0
 
-  /* Configure the accelerometer LPF main parameters */
-  LSM303DLHC_AccFilterConfig(&LSM303DLHCFilter_InitStructure);
-}
+#define GPIO_SPEED_MEDIUM 1
+#define GPIO_SPEED_LOW 0
+#define GPIO_SPEED_HIGH 3
 
-void Demo_CompassReadAcc(float* pfData)
-{
-  int16_t pnRawData[3];
-  uint8_t ctrlx[2];
-  uint8_t buffer[6], cDivider;
-  uint8_t i = 0;
-  float LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_2g;
-  
-  /* Read the register content */
-  LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG4_A, ctrlx,2);
-  LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_X_L_A, buffer, 6);
-   
-  if(ctrlx[1]&0x40)
-    cDivider=64;
-  else
-    cDivider=16;
+extern void gpio_set_mode(GPIO_TypeDef *g,int n,int mode);
 
-  /* check in the control register4 the data alignment*/
-  if(!(ctrlx[0] & 0x40) || (ctrlx[1] & 0x40)) /* Little Endian Mode or FIFO mode */
-  {
-    for(i=0; i<3; i++)
-    {
-      pnRawData[i]=((int16_t)((uint16_t)buffer[2*i+1] << 8) + buffer[2*i])/cDivider;
-    }
-  }
-  else /* Big Endian Mode */
-  {
-    for(i=0; i<3; i++)
-      pnRawData[i]=((int16_t)((uint16_t)buffer[2*i] << 8) + buffer[2*i+1])/cDivider;
-  }
-  /* Read the register content */
-  LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG4_A, ctrlx,2);
+extern void gpio_set_af(GPIO_TypeDef *g,int n,int af, int otype, int pupd, int speed);
 
 
-  if(ctrlx[1]&0x40)
-  {
-    /* FIFO mode */
-    LSM_Acc_Sensitivity = 0.25;
-  }
-  else
-  {
-    /* normal mode */
-    /* switch the sensitivity value set in the CRTL4*/
-    switch(ctrlx[0] & 0x30)
-    {
-    case LSM303DLHC_FULLSCALE_2G:
-      LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_2g;
-      break;
-    case LSM303DLHC_FULLSCALE_4G:
-      LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_4g;
-      break;
-    case LSM303DLHC_FULLSCALE_8G:
-      LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_8g;
-      break;
-    case LSM303DLHC_FULLSCALE_16G:
-      LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_16g;
-      break;
-    }
-  }
+void i2cInit() {
+  RCC->APB1ENR |= RCC_APB1ENR_I2C1EN;
+  RCC->AHBENR |= RCC_AHBENR_GPIOBEN;
+  // configure GPIOPB6 and 7
+  gpio_set_af(GPIOB,6,4,GPIO_OUTPUT_PUSH_PULL,GPIO_PULL_DOWN,GPIO_SPEED_LOW);
+  gpio_set_af(GPIOB,7,4,GPIO_OUTPUT_PUSH_PULL,GPIO_PULL_DOWN,GPIO_SPEED_LOW);
 
-  /* Obtain the mg value for the three axis */
-  for(i=0; i<3; i++)
-  {
-    pfData[i]=(float)pnRawData[i]/LSM_Acc_Sensitivity;
-  }
+  I2C1->TIMINGR = 0x00902025;
+  I2C1->CR1 |= I2C_CR1_PE;
 
 }
 
+#define I2C_CR2_CLEAR_MASK ~(I2C_CR2_SADD | I2C_CR2_NBYTES | I2C_CR2_RELOAD | I2C_CR2_AUTOEND | I2C_CR2_RD_WRN | I2C_CR2_START | I2C_CR2_STOP)
 
+void i2cWrite(int address, int reg, int val) {
+    while(I2C1->ISR & I2C_ISR_BUSY);
+    I2C1->CR2 = (I2C1->CR2 & I2C_CR2_CLEAR_MASK) | I2C_CR2_RELOAD | I2C_CR2_START | (1<<16) | address;
+    while(!(I2C1->ISR & I2C_ISR_TXIS));
+    I2C1->TXDR=reg;
+    while(!(I2C1->ISR & I2C_ISR_TCR));
+    I2C1->CR2 = (I2C1->CR2 & I2C_CR2_CLEAR_MASK) | I2C_CR2_AUTOEND | (1<<16) | address;
+    while(!(I2C1->ISR & I2C_ISR_TXIS));
+    I2C1->TXDR=val;
+    while(!(I2C1->ISR & I2C_ISR_STOPF));
+    I2C1->ICR = I2C_ICR_STOPCF;
+}
+
+void i2cRead(int address, int reg, uint8_t *data, int nbytes) {
+    while(I2C1->ISR & I2C_ISR_BUSY);
+    I2C1->CR2 = (I2C1->CR2 & I2C_CR2_CLEAR_MASK) | I2C_CR2_START | (1<<16) | address;
+    while(!(I2C1->ISR & I2C_ISR_TXIS));
+    if(nbytes>1) reg |= 0x80;
+    I2C1->TXDR=reg;
+    while(!(I2C1->ISR & I2C_ISR_TC));
+    I2C1->CR2 = (I2C1->CR2 & I2C_CR2_CLEAR_MASK) | I2C_CR2_AUTOEND | I2C_CR2_START | I2C_CR2_RD_WRN | (nbytes<<16) | address;
+    while(nbytes--) {
+        while(!(I2C1->ISR & I2C_ISR_RXNE));
+        *data++=I2C1->RXDR;
+    }
+    while(!(I2C1->ISR & I2C_ISR_STOPF));
+    I2C1->ICR = I2C_ICR_STOPCF;
+}
+void MemsConfig(void)
+{
+  i2cInit();
+  i2cWrite(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG1_A,LSM303DLHC_NORMAL_MODE | LSM303DLHC_ODR_50_HZ | LSM303DLHC_AXES_ENABLE);
+  i2cWrite(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG4_A,LSM303DLHC_FULLSCALE_2G | LSM303DLHC_BlockUpdate_Continous | LSM303DLHC_BLE_LSB | LSM303DLHC_HR_ENABLE);
+  i2cWrite(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG2_A,LSM303DLHC_HPM_NORMAL_MODE | LSM303DLHC_HPFCF_16 | LSM303DLHC_HPF_AOI1_DISABLE | LSM303DLHC_HPF_AOI2_DISABLE);
+ 
+  i2cWrite(MAG_I2C_ADDRESS, LSM303DLHC_CRA_REG_M, LSM303DLHC_TEMPSENSOR_ENABLE | LSM303DLHC_ODR_30_HZ);
+  i2cWrite(MAG_I2C_ADDRESS, LSM303DLHC_CRB_REG_M, LSM303DLHC_FS_8_1_GA);
+  i2cWrite(MAG_I2C_ADDRESS, LSM303DLHC_MR_REG_M,LSM303DLHC_CONTINUOS_CONVERSION);
+}
+
+void ReadAccelerometer(int16_t * data) {  
+    i2cRead(ACC_I2C_ADDRESS, LSM303DLHC_OUT_X_L_A, (uint8_t *)data, 6);
+}
+
+void ReadMagnetometer(int16_t * data) {
+    i2cRead(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_L_M, (uint8_t *)data, 1);
+    i2cRead(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_H_M, (uint8_t *)data+1, 1);
+    i2cRead(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_L_M, (uint8_t *)data+2, 1);
+    i2cRead(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_H_M, (uint8_t *)data+3, 1);
+    i2cRead(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_L_M, (uint8_t *)data+4, 1);
+    i2cRead(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_H_M, (uint8_t *)data+5, 1);
+}
+
+
+