Oleg Goncharovskiy / LSM303D_my

compas and acc for my stuents

LSM303D_my.cpp@0:034b0a5fc70a, 2020-07-15 (annotated)

Committer:
docent
Date:
Wed Jul 15 07:31:33 2020 +0000
Revision:
0:034b0a5fc70a
for my students

Who changed what in which revision?

UserRevisionLine numberNew contents of line
docent 0:034b0a5fc70a 1 #include "LSM303D_my.h"
docent 0:034b0a5fc70a 2
docent 0:034b0a5fc70a 3 char EcompLSM303D_GetID(I2C *ecomp)
docent 0:034b0a5fc70a 4 {
docent 0:034b0a5fc70a 5 char data;
docent 0:034b0a5fc70a 6 data=WHO_AM_I;
docent 0:034b0a5fc70a 7 ecomp->write(I2C_ADDR,&data, 1,1); // 1-no stop
docent 0:034b0a5fc70a 8 ecomp->read(I2C_ADDR,&data, 1,0);
docent 0:034b0a5fc70a 9 return data;
docent 0:034b0a5fc70a 10 }
docent 0:034b0a5fc70a 11 //-------------------------------
docent 0:034b0a5fc70a 12 void EcompLSM303D_Ini(I2C *ecomp)
docent 0:034b0a5fc70a 13 {
docent 0:034b0a5fc70a 14 char data_write[2];
docent 0:034b0a5fc70a 15
docent 0:034b0a5fc70a 16 data_write[0]=CTRL7; //enable
docent 0:034b0a5fc70a 17 data_write[1]=0;
docent 0:034b0a5fc70a 18 ecomp->write(I2C_ADDR,data_write, 2,0);
docent 0:034b0a5fc70a 19
docent 0:034b0a5fc70a 20 //! high resolution(6), Magnetic data rate configuration =6.25 Hz(4)=160ms 25Hz(c)=40ms
docent 0:034b0a5fc70a 21 data_write[0]=CTRL5;
docent 0:034b0a5fc70a 22 data_write[1]=0x6c;//0x64;
docent 0:034b0a5fc70a 23 ecomp->write(I2C_ADDR,data_write, 2,0);
docent 0:034b0a5fc70a 24
docent 0:034b0a5fc70a 25 //! Magnetic full-scale (6 +-12gaus)
docent 0:034b0a5fc70a 26 data_write[0]=CTRL6;
docent 0:034b0a5fc70a 27 data_write[1]=0x60;
docent 0:034b0a5fc70a 28 ecomp->write(I2C_ADDR,data_write, 2,0);
docent 0:034b0a5fc70a 29
docent 0:034b0a5fc70a 30 //!update after read, and all axes of acceleration enabled at 25Hz - 0x41
docent 0:034b0a5fc70a 31 //!update continuos, and all axes of acceleration enabled at 25Hz - 0x47
docent 0:034b0a5fc70a 32 data_write[0]=CTRL1;
docent 0:034b0a5fc70a 33 data_write[1]=0x47;
docent 0:034b0a5fc70a 34 ecomp->write(I2C_ADDR,data_write, 2,0);
docent 0:034b0a5fc70a 35
docent 0:034b0a5fc70a 36 //! 50Hz anti-alias, +/- 16g, no self-test, (SPI 3-wire)
docent 0:034b0a5fc70a 37 data_write[0]=CTRL2;
docent 0:034b0a5fc70a 38 data_write[1]=0xe1;
docent 0:034b0a5fc70a 39 ecomp->write(I2C_ADDR,data_write, 2,0);
docent 0:034b0a5fc70a 40 }
docent 0:034b0a5fc70a 41
docent 0:034b0a5fc70a 42 //-----------------------------------------------
docent 0:034b0a5fc70a 43 void EcompLSM303D_Get_M_Axis(I2C *ecomp,int16_t* m)
docent 0:034b0a5fc70a 44 {
docent 0:034b0a5fc70a 45 char data_write[2];
docent 0:034b0a5fc70a 46 char buffer[6];
docent 0:034b0a5fc70a 47 data_write[0]=OUT_X_L_M|0x80;
docent 0:034b0a5fc70a 48 ecomp->write(I2C_ADDR,data_write, 1,1);
docent 0:034b0a5fc70a 49 ecomp->read(I2C_ADDR,buffer, 6,0);
docent 0:034b0a5fc70a 50 m[0]=*((int16_t*)&buffer[0]);
docent 0:034b0a5fc70a 51 m[1]=*((int16_t*)&buffer[2]);
docent 0:034b0a5fc70a 52 m[2]=*((int16_t*)&buffer[4]);
docent 0:034b0a5fc70a 53 }
docent 0:034b0a5fc70a 54
docent 0:034b0a5fc70a 55 //-------------------------------------------------
docent 0:034b0a5fc70a 56 void EcompLSM303D_Get_A_Axis(I2C *ecomp,double* acc)
docent 0:034b0a5fc70a 57 {
docent 0:034b0a5fc70a 58 char data_write[2];
docent 0:034b0a5fc70a 59 char buffer[6];
docent 0:034b0a5fc70a 60 int16_t a[3];
docent 0:034b0a5fc70a 61 data_write[0]=OUT_X_L_A|0x80;
docent 0:034b0a5fc70a 62 ecomp->write(I2C_ADDR,data_write, 1,1);
docent 0:034b0a5fc70a 63 ecomp->read(I2C_ADDR,buffer, 6,0);
docent 0:034b0a5fc70a 64 a[0]=*((int16_t*)&buffer[0]);
docent 0:034b0a5fc70a 65 a[1]=*((int16_t*)&buffer[2]);
docent 0:034b0a5fc70a 66 a[2]=*((int16_t*)&buffer[4]);
docent 0:034b0a5fc70a 67 acc[0]=(double)a[0]*0.061;//0.000 061
docent 0:034b0a5fc70a 68 acc[1]=(double)a[1]*0.061;
docent 0:034b0a5fc70a 69 acc[2]=(double)a[2]*0.0061;
docent 0:034b0a5fc70a 70 }
docent 0:034b0a5fc70a 71
docent 0:034b0a5fc70a 72 //-----------------------------------------
docent 0:034b0a5fc70a 73 uint16_t CalculateBearing(int xi, int yi){
docent 0:034b0a5fc70a 74 double a;
docent 0:034b0a5fc70a 75
docent 0:034b0a5fc70a 76 a=180*atan2((double)yi,(double)xi)/M_PI;
docent 0:034b0a5fc70a 77 if(a < 0)
docent 0:034b0a5fc70a 78 a += 360;
docent 0:034b0a5fc70a 79 return (uint16_t)floor(a);
docent 0:034b0a5fc70a 80 }