Revision 0:034b0a5fc70a, committed 2020-07-15

Comitter:

docent

Date:

Wed Jul 15 07:31:33 2020 +0000

Commit message:

for my students

Changed in this revision

--- /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); 
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303D_my.h	Wed Jul 15 07:31:33 2020 +0000
@@ -0,0 +1,47 @@
+#ifndef __LSM303D_MY_H
+#define __LSM303D_MY_H
+
+#include "mbed.h"
+
+#define M_PI 3.14159265358979323846
+//#define RADIAN  M_PI/180.0
+
+//---------------------ecompas/accelerometer--------------- 
+#define I2C_ADDR   0x3a
+#define  WHO_AM_I  0x0F //ID=0x49
+#define  CTRL1     0x20
+#define  CTRL2     0x21
+//Linear acceleration FS (full-scale) in CTRL2:
+//       ±2g - 0.061 mg/LSB default
+//       ±4g 0.122
+//       ±6g 0.183
+//       ±8g 0.244
+//       ±16g 0.732
+#define  CTRL3      0x22
+#define  CTRL4      0x23
+#define  CTRL5      0x24
+#define  CTRL6      0x25
+//Magnetic FS  (full-scale) in CTRL6:
+//       ±2gauss 0.080 mgauss/LSB
+//       ±4gauss 0.160
+//       ±8gauss 0.320
+//       ±12gauss 0.479
+#define  CTRL7       0x26
+
+#define  STATUS_M    0x07
+#define  OUT_X_L_M   0x08 
+#define  OUT_X_H_M   0x09 
+#define  OUT_Y_L_M   0x0A 
+#define  OUT_Y_H_M   0x0B 
+#define  OUT_Z_L_M   0x0C 
+#define  OUT_Z_H_M   0x0D 
+#define  OUT_X_L_A   0x28
+#define  OUT_Y_L_A   0x2a
+#define  OUT_Z_L_A   0x2c
+
+char EcompLSM303D_GetID(I2C *ecomp);
+void EcompLSM303D_Ini(I2C *ecomp);
+void EcompLSM303D_Get_M_Axis(I2C *ecomp,int16_t* m);
+void EcompLSM303D_Get_A_Axis(I2C *ecomp,double* a);
+uint16_t CalculateBearing(int  x,  int  y);
+#endif