Diff: LSM303D_my.cpp

Revision:

0:034b0a5fc70a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303D_my.cpp	Wed Jul 15 07:31:33 2020 +0000
@@ -0,0 +1,80 @@
+#include "LSM303D_my.h"
+
+char EcompLSM303D_GetID(I2C *ecomp)
+{
+  char data;
+  data=WHO_AM_I;
+  ecomp->write(I2C_ADDR,&data, 1,1); // 1-no stop
+  ecomp->read(I2C_ADDR,&data, 1,0);  
+  return data;   
+}         
+//-------------------------------
+void EcompLSM303D_Ini(I2C *ecomp)
+{
+    char data_write[2];
+
+       data_write[0]=CTRL7;  //enable
+       data_write[1]=0;
+       ecomp->write(I2C_ADDR,data_write, 2,0);     
+       
+//! high resolution(6), Magnetic data rate configuration =6.25 Hz(4)=160ms 25Hz(c)=40ms
+       data_write[0]=CTRL5;  
+       data_write[1]=0x6c;//0x64;
+       ecomp->write(I2C_ADDR,data_write, 2,0);  
+         
+//! Magnetic full-scale (6 +-12gaus)  
+       data_write[0]=CTRL6;  
+       data_write[1]=0x60;
+       ecomp->write(I2C_ADDR,data_write, 2,0); 
+       
+//!update after read, and all axes of acceleration enabled at 25Hz  - 0x41
+//!update continuos, and all axes of acceleration enabled at 25Hz  - 0x47
+       data_write[0]=CTRL1;  
+       data_write[1]=0x47;
+       ecomp->write(I2C_ADDR,data_write, 2,0); 
+       
+//! 50Hz anti-alias, +/- 16g, no self-test, (SPI 3-wire)
+       data_write[0]=CTRL2;  
+       data_write[1]=0xe1;
+       ecomp->write(I2C_ADDR,data_write, 2,0); 
+}
+
+//-----------------------------------------------
+void EcompLSM303D_Get_M_Axis(I2C *ecomp,int16_t* m)
+{
+    char data_write[2];
+    char  buffer[6];
+    data_write[0]=OUT_X_L_M|0x80;
+    ecomp->write(I2C_ADDR,data_write, 1,1); 
+    ecomp->read(I2C_ADDR,buffer, 6,0);
+    m[0]=*((int16_t*)&buffer[0]);
+    m[1]=*((int16_t*)&buffer[2]);
+    m[2]=*((int16_t*)&buffer[4]);  
+}
+
+//-------------------------------------------------
+void EcompLSM303D_Get_A_Axis(I2C *ecomp,double* acc)
+{
+    char data_write[2];
+    char  buffer[6];
+    int16_t a[3];
+    data_write[0]=OUT_X_L_A|0x80;
+    ecomp->write(I2C_ADDR,data_write, 1,1); 
+    ecomp->read(I2C_ADDR,buffer, 6,0);
+    a[0]=*((int16_t*)&buffer[0]);
+    a[1]=*((int16_t*)&buffer[2]);
+    a[2]=*((int16_t*)&buffer[4]);
+    acc[0]=(double)a[0]*0.061;//0.000 061
+    acc[1]=(double)a[1]*0.061;
+    acc[2]=(double)a[2]*0.0061;      
+}
+
+//-----------------------------------------
+uint16_t CalculateBearing(int xi,  int yi){
+double a;
+
+    a=180*atan2((double)yi,(double)xi)/M_PI;
+    if(a < 0)
+      a += 360;
+    return (uint16_t)floor(a); 
+}