Diff: main.c

Revision:

2:742027e49b6f

Parent:

1:c92d1851a036

--- a/main.c	Wed May 02 01:56:42 2018 +0000
+++ b/main.c	Wed May 02 04:06:26 2018 +0000
@@ -1,6 +1,8 @@
 
 #include <stm32f3_discovery.h>
 #include <stm32f3_discovery_lsm303dlhc.h>
+#include <stdio.h>
+#include <math.h>
 
 #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)
 
@@ -37,7 +39,7 @@
   RCC->APB1ENR |= RCC_APB1ENR_I2C1EN;
   // configure GPIOPB6 and 7
   RCC->AHBENR |= RCC_AHBENR_GPIOBEN;
-  GPIOB->AFR[0] = (GPIOC->AFR[0] & 0x00ffffff) | 0x44000000; // alt funcs for GPIOB6 and 7 are 4
+  GPIOB->AFR[0] = (GPIOB->AFR[0] & 0x00ffffff) | 0x44000000; // alt funcs for GPIOB6 and 7 are 4
   GPIOB->OSPEEDR |= 0xf000; // speed high
   GPIOB->OTYPER &= ~0xc0; // output type pp
   GPIOB->MODER = (GPIOB->MODER & ~0xf000) | 0xa000; // mode af (10)
@@ -65,12 +67,12 @@
 }
 
 void ReadMagnetometer(int16_t * data) {
-	i2cRead(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_L_M, (uint8_t *)data, 6);
-	i2cRead(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_H_M, (uint8_t *)data+1, 6);
-	i2cRead(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_L_M, (uint8_t *)data+2, 6);
-	i2cRead(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_H_M, (uint8_t *)data+3, 6);
-	i2cRead(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_L_M, (uint8_t *)data+4, 6);
-	i2cRead(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_H_M, (uint8_t *)data+5, 6);
+	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);
 }
 
 int ReadTemperature() {
@@ -109,9 +111,12 @@
     	
     	ReadAccelerometer(acc);
     	ReadMagnetometer(mag);
+    	float heading = atan2(mag[1], mag[0]);
+        heading=heading * 180 / 3.14159265359f;
+        if (heading < 0) heading +=360;
     	temp=ReadTemperature();
     	printf("Acc: %d %d %d\n",acc[0]/16,acc[1]/16,acc[2]/16);
-    	printf("Mag: %d %d %d %d\n",mag[0],mag[1],mag[2],temp);
+    	printf("Mag: %d %d %d %d %f\n",mag[0],mag[1],mag[2],temp, heading);
     	GPIOE->BSRR=b<<(8+16);
     	b=0;
     	if(acc[1]<-TH) {