Compass with PWM on the stm32f3discovery debug board

The idea of ​​compass implementation on stm32 with PWM (wide pulse modulation) is such a feature when the intensity between adjacent LEDs changes depending on the angle of rotation of the compass. But first you just need to code the usual compass (just to point to the north).

All work was done in CooCox - the Eclipse programming environment (quite buggy). It was possible to use the STM32Cub MX - a much more convenient program, but difficult (the authors did not figure it out).

In order to work with such buns as an accelerometer, a magnetometer (yes, all this is in a small chip on stm-ke), you need to connect a lot of third-party libraries (in fact, the authors did not want to write a huge amount of code with pens).

Section include:
#include "stm32f30x.h" - for working with a microcontroller;
#include "stm32f30x_gpio.h" - for working with peripherals (LEDs);
#include "stm32f30x_i2c.h" - for data exchange between the accelerometer-magnetrometer and the microcircuit;
#include "stm32f30x_rcc.h" - to supply power to the periphery;
#include "stm32f3_discovery_lsm303dlhc.h" - a bun to work with an accelerometer-magnetrometer;
#include "stm32f30x_exti.h" - necessary;
#include "stm32f30x_syscfg.h" - also necessary;
#include <stdio.h> - functions from C;
#include "math.h" - for mat. functions (ala sin, cos);
#include "stm32f3_discovery.h" - to facilitate robots with the board;
#include "stm32f30x_tim.h" - timers.
')
After several days of searching the Internet for this good, it was necessary to connect everything to CooCox - and that was where the first problems began.

With standard libraries a la stm32f30x_gpio.h, stm32f30x_tim.h everything was prosaic, but with third-party (stm32f3_discovery_lsm303dlhc and stm32f3_discovery) there was a little difficulty. It was necessary to manually register the paths and throw them into the necessary folders, and there are many of them and it is not clear which ones to use.

In the hierarchical structure of the Project it was necessary to throw them into cmis_lib / include (header). The second way: cmis_lib / source (with - files). But after that the compiler will continue to swear. To make compiler use, you need to duplicate it in two libraries, header files:

#include "stm32f30x.h"
#include "stm32f30x_exti.h"
#include "stm32f30x_syscfg.h"
#include "stm32f30x_gpio.h"
#include "stm32f30x_i2c.h"
#include "stm32f30x_rcc.h"
#include "stm32f30x_usart.h"
#include "stm32f30x_spi.h"
#include "stm32f30x_misc.h"

Now you can start working ...

Since after several days, nights, weeks, cups of coffee (with snickers) a compass was born (you can go on a hike, just do not forget the batteries!), With a huge amount of govnokod, which will not be shown below because of its large size, but a logical structure is described (for those who want to see this coding, at the end there will be a download link). Well, maybe a little code yes.

To realize the compass you need to work with the LSM303DLHC integrated circuit board. It has everything to work with a future compass: a magnetrometer (for reading magnetic field data) and an accelerometer (for correcting the readings due to the inclination of the board).

First you need to calculate the azimuth between the direction of the vector of the earth’s magnetic field and the axis X of the magnetometer. To calculate the azimuth, data on the magnitude of the magnetic field along the X and Y axes of the magnetometer will be required. These are the projections of the vector of the earth’s magnetic field on the X and Y axes. Then, to calculate the angle, you can recall the geometry lessons and apply the simplest formula: arctg (Y / X), where Y and X are the projection values ​​on the Y and X axes, respectively.

These manipulations are suitable for a completely flat surface. When there are no deviations in different angles. But the hand of a little man tends to shake and knows absolutely nothing about a flat surface. To solve this problem, an accelerometer was used, which records this slope.

Also in the project can not do without data on the acceleration for each of the axes. They will allow you to calculate the angles of roll and pitch, which will subsequently be taken into account when calculating the direction to the north (read Wikipedia). Subtracting data from the magnetometer and accelerometer occurs by using the functions described below:

Listing 1. Function to read accelerometer data

void Demo_CompassReadAss(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; 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; if(!(ctrlx[0] & 0x40) || (ctrlx[1] & 0x40)) { for(i=0; i<3; i++) { pnRawData[i]=((int16_t)((uint16_t)buffer[2*i+1] << 8) + buffer[2*i])/cDivider;//    (  - ) } } else { for(i=0; i<3; i++) pnRawData[i]=((int16_t)((uint16_t)buffer[2*i] << 8) + buffer[2*i+1])/cDivider; } LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG4_A, ctrlx,2); if(ctrlx[1]&0x40) { LSM_Acc_Sensitivity = 0.25; } else { 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; } } for(i=0; i<3; i++) { pfData[i]=(float)pnRawData[i]/LSM_Acc_Sensitivity;//          } }<source> <b> 2.     </b> <source>void Demo_CompassRegMag(float *pfData) { static int16_t buffer[3]={0}; uint8_t *ptr=buffer; uint8_t CTRLB=0; uint16_t Magn_Sensitivity_XY=0, Magn_Sensitivity_Z=0; uint8_t i=0; float fpfData[3]={0.0f}; LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_CRB_REG_M, &CTRLB, 1);//   LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_H_M, ptr, 1); LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_L_M, ptr+1, 1); LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_H_M, ptr+2, 1); LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_L_M, ptr+3, 1); LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_H_M, ptr+4, 1); LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_L_M, ptr+5, 1); switch(CTRLB & 0xE0)//  { case LSM303DLHC_FS_1_3_GA: Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_1_3Ga; Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_1_3Ga; break; case LSM303DLHC_FS_1_9_GA: Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_1_9Ga; Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_1_9Ga; break; case LSM303DLHC_FS_2_5_GA: Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_2_5Ga; Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_2_5Ga; break; case LSM303DLHC_FS_4_0_GA: Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_4Ga; Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_4Ga; break; case LSM303DLHC_FS_4_7_GA: Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_4_7Ga; Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_4_7Ga; break; case LSM303DLHC_FS_5_6_GA: Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_5_6Ga; Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_5_6Ga; break; case LSM303DLHC_FS_8_1_GA: Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_8_1Ga; Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_8_1Ga; break; } fpfData[0]=(((float)buffer[0])/1000)/Magn_Sensitivity_XY;//     fpfData[1]=(((float)buffer[1])/1000)/Magn_Sensitivity_XY; fpfData[2]=(((float)buffer[2])/1000)/Magn_Sensitivity_Z; for(i=0;i<3;i++) { pfData[i]=fpfData[i]*(-1); //   .    " "   -1 } }<source>         : fNormAcc=sqrt((AccBuffer[0]*AccBuffer[0])+(AccBuffer[1]*AccBuffer[1])+(AccBuffer[2]*AccBuffer[2])); ,           (     ),      (Roll)   (Pitch): fSinRoll = -AccBuffer[1]/ fNormAcc; fCosRoll = sqrt(1.0 - (fSinRoll *fSinRoll)); fSinPitch = AccBuffer[0]/fNormAcc; fCosPitch = sqrt(1.0 -(fSinPitch * fSinPitch));        : RollAng = acos(fCosRoll) * 180/PI; PitchAng = acos(fCosPitch) *180/PI;            180  360 ,      .      ,                .          : fTitledX=MagBuffer[0]*fCosPitch+MagBuffer[2]*fSinPitch; fTitledY=MagBuffer[0]*fSinRoll*fSinPitch+MagBuffer[1]*fCosRoll-MagBuffer[1]*fSinRoll*fCosPitch;   : HeadingValue= 180.0f - (float)((atan2f((float)fTitledY,(float)fTitledX))*180)/PI;//   ,      -.   :       3D .      .       .  .  -.       .      (  Pin-  Pin- ). .        1 (TIM1) -  datasheet,              Pin-  ,     4,   - 8.   datasheet     (TIM3).  : PC6, PC7, PC8, PC9.    ,      .      user manual.  : led4, led5, led6, led9. ,  Pin-: PE8, PE10, PE12, PE15.     :    Pin-   Pin-       ( TIM1  TIM3): PE8-PC6 PE10-PC7 PE12-PC8 PE15-PC9.        1.  3  . <source>{ GPIO_StructInit(&gpio2); gpio2.GPIO_Mode = GPIO_Mode_AF; gpio2.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9; gpio2.GPIO_Speed=GPIO_Speed_Level_1; gpio2.GPIO_OType=GPIO_OType_PP; gpio2.GPIO_PuPd=GPIO_PuPd_NOPULL; RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOC,ENABLE); GPIO_Init(GPIOC,&gpio2); GPIO_PinAFConfig(GPIOC,GPIO_PinSource6,GPIO_AF_2);//PC6-PE8(LD4) GPIO_PinAFConfig(GPIOC,GPIO_PinSource7,GPIO_AF_2);//PC7-PE10(LED5) GPIO_PinAFConfig(GPIOC,GPIO_PinSource8,GPIO_AF_2);//PC8-PE12(LED9) GPIO_PinAFConfig(GPIOC,GPIO_PinSource9,GPIO_AF_2);//PC9-PE15(LED6) // RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE); //  TIM_TimeBaseStructInit(&timer3); timer3.TIM_Prescaler = 720; timer3.TIM_Period = 100; TIM_TimeBaseInit(TIM3, &timer3); TIM_OCStructInit(&timer3_oc); timer3_oc.TIM_OCMode=TIM_OCMode_PWM1; timer3_oc.TIM_OutputState=TIM_OutputState_Enable; TIM_OC1Init(TIM3, &timer3_oc);//LED4 TIM_OC2Init(TIM3, &timer3_oc);//LED5 TIM_OC3Init(TIM3, &timer3_oc);//LED9 TIM_OC4Init(TIM3, &timer3_oc);//LED6 TIM3->CCR1=10; // . TIM_SetCompare1(TIM3,0); TIM_SetCompare2(TIM3,0); TIM_SetCompare3(TIM3,0); TIM_SetCompare4(TIM3,0); TIM_CCxCmd(TIM3,TIM_Channel_1,TIM_CCx_Enable);//     LED4  TIM_CCxCmd(TIM3,TIM_Channel_2,TIM_CCx_Enable);//     LED5  TIM_CCxCmd(TIM3,TIM_Channel_3,TIM_CCx_Enable);//     LED9  TIM_CCxCmd(TIM3,TIM_Channel_4,TIM_CCx_Enable);//     LED8  TIM_Cmd(TIM3,ENABLE); }<source>        -   8          .    :     .          HadingValue.  ,    (HadingValue)    .  ,     0  45 .   - . void onLED(void) { if(HeadingValue < 0) { HeadingValue=HeadingValue+360; } if ((fRollAng <= 40.0f) && (fPitchAng <= 40.0f))// LED10 { if (((HeadingValue < 25.0f)&&(HeadingValue >= 0.0f))||((HeadingValue >=340.0f)&&(HeadingValue <= 360.0f))) { TIM_SetCompare1(TIM1,0); TIM_SetCompare2(TIM1,0); TIM_SetCompare3(TIM1,((int)(45.0f-HeadingValue))*2);//LED10 TIM_SetCompare4(TIM1,((int)(HeadingValue-0))*2);//led8 //---------------------- TIM_SetCompare1(TIM3,0); TIM_SetCompare2(TIM3,0); TIM_SetCompare3(TIM3,0); TIM_SetCompare4(TIM3,0); } /*..... */ }        (  1 )    ,         .          TIM2. <source>//  void timer_init(void) { RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); TIM_TimeBaseStructInit(&timer); timer.TIM_Prescaler=720; timer.TIM_Period=100;//   2  TIM_TimeBaseInit(TIM2,&timer); TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE); TIM_Cmd(TIM2,ENABLE); NVIC_EnableIRQ(TIM2_IRQn); } //  void TIM2_IRQHandler(void) { calcangle(); onLED(); TIM_ClearFlag(TIM2, TIM_FLAG_Update); }<source>          main: <source>uint16_t intensity=0; TIM_OCInitTypeDef timer_oc; RCC_ClocksTypeDef RCC_Clocks; TIM_TimeBaseInitTypeDef timer; TIM_TimeBaseInitTypeDef timer3; TIM_OCInitTypeDef timer3_oc; GPIO_InitTypeDef gpio; GPIO_InitTypeDef gpio2; float fNormAcc=0.0f; float HeadingValue=0.0f; float fSinRoll=0.0f,fCosRoll=0.0f; float fSinPitch=0.0f, fCosPitch=0.0f; float fRollAng=0.0f,fPitchAng=0.0f; float fTitledX=0.0f, fTitledY=0.0f; uint32_t TimingDelay = 0; const float PI=3.14; float MagBuffer[3]={0.0f}; float AccBuffer[3]={0.0f}; int main(void) { //  RCC_GetClocksFreq(&RCC_Clocks); SysTick_Config(RCC_Clocks.HCLK_Frequency / 100); //  timer3_init();// 1  3  timer1_init(); //    Demo_CompassConfig(); //calcangle(); //onLED(); //  timer_init(); while(1) { } }<source>   !   : https://yadi.sk/d/s7vAizVKh3jFx Datasheet: https://yadi.sk/i/kmUrRQzeh3jJ9 User Manual: https://yadi.sk/i/IxbW4Gn-h3jKa