X SZ
LSM303DLHC Full Driver: Readings For Acc, Mag and Temp; Class Method for frequently-used 13 sensor parameters; Class Method to direct modify registers; Support Calibration (offset+scale);
Diff: LSM303DLHC.cpp
- Revision:
- 0:7864abfabe2f
- Child:
- 1:e68ce5025dad
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303DLHC.cpp Mon Jun 06 07:40:45 2016 +0000
@@ -0,0 +1,200 @@
+#include "mbed.h"
+#include "LSM303DLHC.h"
+
+LSM303DLHC::LSM303DLHC(PinName sda, PinName scl) : i2c(sda, scl){
+ i2c.frequency(400000);
+
+ HPF_state = false; // reg default value corresponds to this
+
+ acc_offset[0] = 0;
+ acc_offset[1] = 0;
+ acc_offset[2] = 0;
+ acc_scale[0] = 1;
+ acc_scale[1] = 1;
+ acc_scale[2] = 1;
+
+ mag_offset[0] = 0;
+ mag_offset[1] = 0;
+ mag_offset[2] = 0;
+ mag_scale[0] = 1;
+ mag_scale[1] = 1;
+ mag_scale[2] = 1;
+
+ temp_offset[0] = 0;
+ temp_scale[0] = 1;
+
+ // CUSTOM THE INITIAL CTRL FOR YOURSELF
+ ACtrl(LP_OFF); // ACC Normal Power Mode
+ ACtrl(ODR3); // ACC ON and Date Rate 25Hz
+ ACtrl(HPF_ON); // ACC internal HPF In Use
+ ACtrl(HPF_CF0); // ACC HPF Cutoff Freq = option 0
+ ACtrl(HPF_NORM); // ACC HPF Model = Normal
+ ACtrl(BDU_CONT); // ACC data continuous
+ ACtrl(G4); // ACC Range +/-4g
+ ACtrl(HIGH_R); // ACC in High Prec
+ MCtrl(DR4); // MAG DR 15Hz
+ MCtrl(GN4); // MAG GN 4.0Gauss
+ MCtrl(MD_CONT); // MAG ON and MD Continuous
+ TCtrl(TEMP_ON); // TEMP ON
+}
+
+void LSM303DLHC::GetAcc(float arr[3]){
+ data[0] = OUT_X_L_A | (1 << 7); // MSB=1 to read multiple bytes
+ i2c.write(ACC_ADDRESS, data, 1);
+ i2c.read(ACC_ADDRESS, data, 6);
+
+ // return the raw acceleration
+ if (HPF_state == HPF_ON){ // HPF has no offset
+ arr[0] = acc_scale[0]*((short)(data[1]<<8 | data[0])>>4);
+ arr[1] = acc_scale[1]*((short)(data[3]<<8 | data[2])>>4);
+ arr[2] = acc_scale[2]*((short)(data[5]<<8 | data[4])>>4);
+ }else{
+ arr[0] = acc_scale[0]*(acc_offset[0]+((short)(data[1]<<8 | data[0])>>4));
+ arr[1] = acc_scale[1]*(acc_offset[1]+((short)(data[3]<<8 | data[2])>>4));
+ arr[2] = acc_scale[2]*(acc_offset[2]+((short)(data[5]<<8 | data[4])>>4));
+ }
+}
+
+void LSM303DLHC::GetMag(float arr[3]){
+ data[0] = OUT_X_H_M;
+ i2c.write(MAG_ADDRESS, data, 1);
+ i2c.read(MAG_ADDRESS, data, 6);
+ arr[0] = mag_scale[0]*(mag_offset[0]+(short)(data[0]<<8 | data[1]));
+ arr[1] = mag_scale[1]*(mag_offset[1]+(short)(data[4]<<8 | data[5]));
+ arr[2] = mag_scale[2]*(mag_offset[2]+(short)(data[2]<<8 | data[3]));
+}
+
+void LSM303DLHC::GetTemp(float arr[1]){
+ data[0] = TEMP_OUT_H_M;
+ i2c.write(MAG_ADDRESS, data, 1);
+ i2c.read(MAG_ADDRESS, data, 2);
+ arr[0] = temp_scale[0]*(temp_offset[0]+((short)(data[0]<<8 | data[1])>>4));
+}
+
+void LSM303DLHC::ACtrl(ACC_ODR cmd){
+ data[0] = CTRL_REG1_A;
+ i2c.write(ACC_ADDRESS, data, 1);
+ i2c.read(ACC_ADDRESS, &data[1], 1);
+ data[1] = data[1] & (0b00001111) | (cmd<<4);
+ i2c.write(ACC_ADDRESS, data, 2);
+}
+
+void LSM303DLHC::ACtrl(ACC_LPen cmd){
+ data[0] = CTRL_REG1_A;
+ i2c.write(ACC_ADDRESS, data, 1);
+ i2c.read(ACC_ADDRESS, &data[1], 1);
+ data[1] = data[1] & (0b11110111) | (cmd<<3);
+ i2c.write(ACC_ADDRESS, data, 2);
+}
+
+void LSM303DLHC::ACtrl(ACC_HPM cmd){
+ data[0] = CTRL_REG2_A;
+ i2c.write(ACC_ADDRESS, data, 1);
+ i2c.read(ACC_ADDRESS, &data[1], 1);
+ data[1] = data[1] & (0b00111111) | (cmd<<6);
+ i2c.write(ACC_ADDRESS, data, 2);
+}
+
+void LSM303DLHC::ACtrl(ACC_HPCF cmd){
+ data[0] = CTRL_REG2_A;
+ i2c.write(ACC_ADDRESS, data, 1);
+ i2c.read(ACC_ADDRESS, &data[1], 1);
+ data[1] = data[1] & (0b11001111) | (cmd<<4);
+ i2c.write(ACC_ADDRESS, data, 2);
+}
+
+void LSM303DLHC::ACtrl(ACC_FDS cmd){
+ data[0] = CTRL_REG2_A;
+ i2c.write(ACC_ADDRESS, data, 1);
+ i2c.read(ACC_ADDRESS, &data[1], 1);
+ data[1] = data[1] & (0b11110111) | (cmd<<3);
+ i2c.write(ACC_ADDRESS, data, 2);
+ (cmd == HPF_ON)?(HPF_state = true):(HPF_state = false);
+}
+
+void LSM303DLHC::ACtrl(ACC_BDU cmd){
+ data[0] = CTRL_REG4_A;
+ i2c.write(ACC_ADDRESS, data, 1);
+ i2c.read(ACC_ADDRESS, &data[1], 1);
+ data[1] = data[1] & (0b01111111) | (cmd<<7);
+ i2c.write(ACC_ADDRESS, data, 2);
+}
+
+void LSM303DLHC::ACtrl(ACC_FS cmd){
+ data[0] = CTRL_REG4_A;
+ i2c.write(ACC_ADDRESS, data, 1);
+ i2c.read(ACC_ADDRESS, &data[1], 1);
+ data[1] = data[1] & (0b11001111) | (cmd<<4);
+ i2c.write(ACC_ADDRESS, data, 2);
+}
+
+void LSM303DLHC::ACtrl(ACC_HR cmd){
+ data[0] = CTRL_REG4_A;
+ i2c.write(ACC_ADDRESS, data, 1);
+ i2c.read(ACC_ADDRESS, &data[1], 1);
+ data[1] = data[1] & (0b11110111) | (cmd<<3);
+ i2c.write(ACC_ADDRESS, data, 2);
+}
+
+void LSM303DLHC::TCtrl(TEMP_EN cmd){
+ data[0] = CRA_REG_M;
+ i2c.write(MAG_ADDRESS, data, 1);
+ i2c.read(MAG_ADDRESS, &data[1], 1);
+ data[1] = data[1] & (0b01111111) | (cmd<<7);
+ i2c.write(MAG_ADDRESS, data, 2);
+}
+
+void LSM303DLHC::MCtrl(MAG_DR cmd){
+ data[0] = CRA_REG_M;
+ i2c.write(MAG_ADDRESS, data, 1);
+ i2c.read(MAG_ADDRESS, &data[1], 1);
+ data[1] = data[1] & (0b11100011) | (cmd<<2);
+ i2c.write(MAG_ADDRESS, data, 2);
+}
+
+void LSM303DLHC::MCtrl(MAG_GN cmd){
+ data[0] = CRB_REG_M;
+ i2c.write(MAG_ADDRESS, data, 1);
+ i2c.read(MAG_ADDRESS, &data[1], 1);
+ data[1] = data[1] & (0b00011111) | (cmd<<5);
+ i2c.write(MAG_ADDRESS, data, 2);
+}
+
+void LSM303DLHC::MCtrl(MAG_MD cmd){
+ data[0] = MR_REG_M;
+ i2c.write(MAG_ADDRESS, data, 1);
+ i2c.read(MAG_ADDRESS, &data[1], 1);
+ data[1] = data[1] & (0b11111100) | (cmd<<0);
+ i2c.write(MAG_ADDRESS, data, 2);
+}
+
+void LSM303DLHC::WriteReg(int sad, char d[2]){
+ i2c.write(sad, d, 2);
+}
+
+void LSM303DLHC::AccCal(float offset[3], float scale[3]){
+ acc_offset[0] = offset[0];
+ acc_offset[1] = offset[1];
+ acc_offset[2] = offset[2];
+ acc_scale[0] = scale[0];
+ acc_scale[1] = scale[1];
+ acc_scale[2] = scale[2];
+}
+
+void LSM303DLHC::MagCal(float offset[3], float scale[3]){
+ mag_offset[0] = offset[0];
+ mag_offset[1] = offset[1];
+ mag_offset[2] = offset[2];
+ mag_scale[0] = scale[0];
+ mag_scale[1] = scale[1];
+ mag_scale[2] = scale[2];
+}
+
+void LSM303DLHC::TempCal(float offset[1], float scale[1]){
+ temp_offset[0] = offset[0];
+ temp_scale[0] = scale[0];
+}
+
+bool LSM303DLHC::isHPFEn(){
+ return HPF_state;
+}
\ No newline at end of file