Waldemar Dworakowski / LSM303DLHC

Based on https://os.mbed.com/users/Airium/code/module_LSM303DLHC/ Main difference is the usage of the I2C - now it is passed as parameter making cooperative use easier. 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);

LSM303DLHC.cpp@1:4ee6df2df73a, 2020-02-03 (annotated)

Committer:
Pythia
Date:
Mon Feb 03 07:43:48 2020 +0000
Revision:
1:4ee6df2df73a
Parent:
0:a4131de4bddd 
Version working. Temperature sensor provide strange results. Is to be extended with orientation measurement.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Pythia 0:a4131de4bddd 1 #include "mbed.h"
Pythia 0:a4131de4bddd 2 #include "LSM303DLHC.h"
Pythia 0:a4131de4bddd 3
Pythia 0:a4131de4bddd 4 LSM303DLHC::LSM303DLHC(I2C &i2c){
Pythia 0:a4131de4bddd 5
Pythia 0:a4131de4bddd 6 _i2c = &i2c;
Pythia 0:a4131de4bddd 7 // use setting for _i2c->frequency(400000);
Pythia 0:a4131de4bddd 8
Pythia 0:a4131de4bddd 9 // HERE GIVES DEVICE DEFAULT
Pythia 0:a4131de4bddd 10 ACtrl(LP_OFF); // ACC Normal Power Mode
Pythia 0:a4131de4bddd 11 ACtrl(ADR3); // ACC ON and Date Rate 25Hz
Pythia 0:a4131de4bddd 12 ACtrl(XYZ); // ACC XYZ Axis Enable
Pythia 0:a4131de4bddd 13 ACtrl(HPF_ON); // ACC internal HPF IN USE
Pythia 0:a4131de4bddd 14 ACtrl(HPF_CF0); // ACC HPF Cutoff Freq = option 0
Pythia 0:a4131de4bddd 15 ACtrl(HPF_NORM_R); // ACC HPF Model = Normal
Pythia 0:a4131de4bddd 16 ACtrl(BDU_CONT); // ACC data continuous
Pythia 0:a4131de4bddd 17 ACtrl(G4); // ACC Range +/-4g
Pythia 0:a4131de4bddd 18 ACtrl(HIGH_R); // ACC in High Prec
Pythia 0:a4131de4bddd 19 MCtrl(MDR4); // MAG DR 15Hz
Pythia 0:a4131de4bddd 20 MCtrl(GN4); // MAG GN 4.0Gauss
Pythia 0:a4131de4bddd 21 MCtrl(MD_CONT); // MAG ON and MD Continuous
Pythia 0:a4131de4bddd 22 TCtrl(TEMP_ON); // TEMP ON
Pythia 0:a4131de4bddd 23
Pythia 0:a4131de4bddd 24 // DEFAULT CALIBRATION PARAMETER
Pythia 0:a4131de4bddd 25 acc_offset[0] = 0;
Pythia 0:a4131de4bddd 26 acc_offset[1] = 0;
Pythia 0:a4131de4bddd 27 acc_offset[2] = 0;
Pythia 0:a4131de4bddd 28 acc_scale[0] = 1;
Pythia 0:a4131de4bddd 29 acc_scale[1] = 1;
Pythia 0:a4131de4bddd 30 acc_scale[2] = 1;
Pythia 0:a4131de4bddd 31
Pythia 0:a4131de4bddd 32 mag_offset[0] = 0;
Pythia 0:a4131de4bddd 33 mag_offset[1] = 0;
Pythia 0:a4131de4bddd 34 mag_offset[2] = 0;
Pythia 0:a4131de4bddd 35 mag_scale[0] = 1;
Pythia 0:a4131de4bddd 36 mag_scale[1] = 1;
Pythia 0:a4131de4bddd 37 mag_scale[2] = 1;
Pythia 0:a4131de4bddd 38
Pythia 0:a4131de4bddd 39 temp_offset[0] = 0;
Pythia 0:a4131de4bddd 40 temp_scale[0] = 1.;
Pythia 0:a4131de4bddd 41 }
Pythia 0:a4131de4bddd 42
Pythia 1:4ee6df2df73a 43 int LSM303DLHC::GetAcc(float arr[3]){
Pythia 1:4ee6df2df73a 44 int result;
Pythia 0:a4131de4bddd 45 data[0] = OUT_X_L_A | (1 << 7); // MSB=1 to read multiple bytes
Pythia 1:4ee6df2df73a 46 result = _i2c->write(ACC_ADDRESS, data, 1);
Pythia 1:4ee6df2df73a 47 if (0 != result)
Pythia 1:4ee6df2df73a 48 {
Pythia 1:4ee6df2df73a 49 printf("\nI2C write issue (%02x)%d\n", MAG_ADDRESS, result);
Pythia 1:4ee6df2df73a 50 return result;
Pythia 1:4ee6df2df73a 51 }
Pythia 1:4ee6df2df73a 52 result = _i2c->read(ACC_ADDRESS, data, 6);
Pythia 1:4ee6df2df73a 53 if (0 != result)
Pythia 1:4ee6df2df73a 54 {
Pythia 1:4ee6df2df73a 55 printf("\nI2C read issue (%02x)%d\n", MAG_ADDRESS, result);
Pythia 1:4ee6df2df73a 56 return result;
Pythia 1:4ee6df2df73a 57 }
Pythia 0:a4131de4bddd 58
Pythia 0:a4131de4bddd 59 int count_x = ((((int)(int8_t)data[1])<<8 | ((uint8_t)data[0])) >> 4);
Pythia 0:a4131de4bddd 60 int count_y = ((((int)(int8_t)data[3])<<8 | ((uint8_t)data[2])) >> 4);
Pythia 0:a4131de4bddd 61 int count_z = ((((int)(int8_t)data[5])<<8 | ((uint8_t)data[4])) >> 4);
Pythia 0:a4131de4bddd 62
Pythia 1:4ee6df2df73a 63 float a_x = acc_scale[0] * (acc_offset[0] + count_x * acc_scale_multiplier);
Pythia 1:4ee6df2df73a 64 float a_y = acc_scale[1] * (acc_offset[1] + count_y * acc_scale_multiplier);
Pythia 1:4ee6df2df73a 65 float a_z = acc_scale[2] * (acc_offset[2] + count_z * acc_scale_multiplier);
Pythia 1:4ee6df2df73a 66 arr[0] = a_x;
Pythia 1:4ee6df2df73a 67 arr[1] = a_y;
Pythia 1:4ee6df2df73a 68 arr[2] = a_z;
Pythia 0:a4131de4bddd 69
Pythia 1:4ee6df2df73a 70 // printf("\nA %02X.%02X %02X.%02X %02X.%02X %04x %04x %04x %7.3f %7.3f %7.3f \n", data[1], data[0], data[3], data[2], data[5], data[4], count_x, count_y, count_z, a_x, a_y, a_z);
Pythia 1:4ee6df2df73a 71
Pythia 1:4ee6df2df73a 72 return 0;
Pythia 0:a4131de4bddd 73 }
Pythia 0:a4131de4bddd 74
Pythia 1:4ee6df2df73a 75 int LSM303DLHC::GetMag(float arr[3]){
Pythia 1:4ee6df2df73a 76 int result;
Pythia 0:a4131de4bddd 77 data[0] = OUT_X_H_M;
Pythia 1:4ee6df2df73a 78 result = _i2c->write(MAG_ADDRESS, data, 1);
Pythia 1:4ee6df2df73a 79 if (0 != result)
Pythia 1:4ee6df2df73a 80 {
Pythia 1:4ee6df2df73a 81 printf("\nI2C write issue (%02x)%d\n", MAG_ADDRESS, result);
Pythia 1:4ee6df2df73a 82 return result;
Pythia 1:4ee6df2df73a 83 }
Pythia 1:4ee6df2df73a 84 result = _i2c->read(MAG_ADDRESS, data, 6);
Pythia 1:4ee6df2df73a 85 if (0 != result)
Pythia 1:4ee6df2df73a 86 {
Pythia 1:4ee6df2df73a 87 printf("\nI2C read issue (%02x)%d\n", MAG_ADDRESS, result);
Pythia 1:4ee6df2df73a 88 return result;
Pythia 1:4ee6df2df73a 89 }
Pythia 1:4ee6df2df73a 90
Pythia 0:a4131de4bddd 91 int count_x = ((int)(int8_t)data[0])<<8 | ((uint8_t)data[1]);
Pythia 0:a4131de4bddd 92 int count_y = ((int)(int8_t)data[4])<<8 | ((uint8_t)data[5]);
Pythia 0:a4131de4bddd 93 int count_z = ((int)(int8_t)data[2])<<8 | ((uint8_t)data[3]);
Pythia 0:a4131de4bddd 94
Pythia 1:4ee6df2df73a 95 float m_x = mag_scale[0] * (mag_offset[0] + count_x * mag_scale_x_multiplier);
Pythia 1:4ee6df2df73a 96 float m_y = mag_scale[1] * (mag_offset[1] + count_y * mag_scale_y_multiplier);
Pythia 1:4ee6df2df73a 97 float m_z = mag_scale[2] * (mag_offset[2] + count_z * mag_scale_z_multiplier);
Pythia 0:a4131de4bddd 98
Pythia 1:4ee6df2df73a 99 arr[0] = m_x;
Pythia 1:4ee6df2df73a 100 arr[1] = m_y;
Pythia 1:4ee6df2df73a 101 arr[2] = m_z;
Pythia 1:4ee6df2df73a 102
Pythia 1:4ee6df2df73a 103 // printf("\nM %02X.%02X %02X.%02X %02X.%02X %04x %04x %04x %7.3f %7.3f %7.3f \n", data[1], data[0], data[3], data[2], data[5], data[4], count_x, count_y, count_z, m_x, m_y, m_z);
Pythia 1:4ee6df2df73a 104
Pythia 1:4ee6df2df73a 105 return 0;
Pythia 0:a4131de4bddd 106 }
Pythia 0:a4131de4bddd 107
Pythia 1:4ee6df2df73a 108 int LSM303DLHC::GetTemp(float arr[1]){
Pythia 1:4ee6df2df73a 109 int result;
Pythia 0:a4131de4bddd 110 data[0] = TEMP_OUT_H_M;
Pythia 1:4ee6df2df73a 111 result = _i2c->write(MAG_ADDRESS, data, 1);
Pythia 1:4ee6df2df73a 112 if (0 != result)
Pythia 1:4ee6df2df73a 113 {
Pythia 1:4ee6df2df73a 114 printf("\nI2C write issue (%02x)%d\n", MAG_ADDRESS, result);
Pythia 1:4ee6df2df73a 115 return result;
Pythia 1:4ee6df2df73a 116 }
Pythia 1:4ee6df2df73a 117 result = _i2c->read(MAG_ADDRESS, data, 2);
Pythia 1:4ee6df2df73a 118 if (0 != result)
Pythia 1:4ee6df2df73a 119 {
Pythia 1:4ee6df2df73a 120 printf("\nI2C read issue (%02x)%d\n", MAG_ADDRESS, result);
Pythia 1:4ee6df2df73a 121 return result;
Pythia 1:4ee6df2df73a 122 }
Pythia 1:4ee6df2df73a 123
Pythia 0:a4131de4bddd 124 int count = (((int)(int8_t)data[0]<<8) | ((uint8_t)data[1]))>>4;
Pythia 1:4ee6df2df73a 125 float temp = count / 8.;
Pythia 1:4ee6df2df73a 126
Pythia 1:4ee6df2df73a 127 // printf("\nT %02X.%02X %04X %d %7.3f\n", data[0], data[1], count, count, temp);
Pythia 1:4ee6df2df73a 128
Pythia 1:4ee6df2df73a 129 arr[0] = temp_scale[0] * (temp_offset[0] + temp);
Pythia 1:4ee6df2df73a 130
Pythia 1:4ee6df2df73a 131 return 0;
Pythia 0:a4131de4bddd 132 }
Pythia 0:a4131de4bddd 133
Pythia 0:a4131de4bddd 134 void LSM303DLHC::ACtrl(ACC_ODR cmd){
Pythia 0:a4131de4bddd 135 data[0] = CTRL_REG1_A;
Pythia 0:a4131de4bddd 136 _i2c->write(ACC_ADDRESS, data, 1);
Pythia 0:a4131de4bddd 137 _i2c->read(ACC_ADDRESS, &data[1], 1);
Pythia 0:a4131de4bddd 138 data[1] = data[1] & (0b00001111) | (cmd<<4);
Pythia 0:a4131de4bddd 139 _i2c->write(ACC_ADDRESS, data, 2);
Pythia 0:a4131de4bddd 140 }
Pythia 0:a4131de4bddd 141
Pythia 0:a4131de4bddd 142 void LSM303DLHC::ACtrl(ACC_LPen cmd){
Pythia 0:a4131de4bddd 143 data[0] = CTRL_REG1_A;
Pythia 0:a4131de4bddd 144 _i2c->write(ACC_ADDRESS, data, 1);
Pythia 0:a4131de4bddd 145 _i2c->read(ACC_ADDRESS, &data[1], 1);
Pythia 0:a4131de4bddd 146 data[1] = data[1] & (0b11110111) | (cmd<<3);
Pythia 0:a4131de4bddd 147 _i2c->write(ACC_ADDRESS, data, 2);
Pythia 0:a4131de4bddd 148 }
Pythia 0:a4131de4bddd 149
Pythia 0:a4131de4bddd 150 void LSM303DLHC::ACtrl(ACC_AXIS cmd){
Pythia 0:a4131de4bddd 151 data[0] = CTRL_REG1_A;
Pythia 0:a4131de4bddd 152 _i2c->write(ACC_ADDRESS, data, 1);
Pythia 0:a4131de4bddd 153 _i2c->read(ACC_ADDRESS, &data[1], 1);
Pythia 0:a4131de4bddd 154 data[1] = data[1] & (0b11111000) | (cmd<<0);
Pythia 0:a4131de4bddd 155 _i2c->write(ACC_ADDRESS, data, 2);
Pythia 0:a4131de4bddd 156 }
Pythia 0:a4131de4bddd 157
Pythia 0:a4131de4bddd 158 void LSM303DLHC::ACtrl(ACC_HPM cmd){
Pythia 0:a4131de4bddd 159 data[0] = CTRL_REG2_A;
Pythia 0:a4131de4bddd 160 _i2c->write(ACC_ADDRESS, data, 1);
Pythia 0:a4131de4bddd 161 _i2c->read(ACC_ADDRESS, &data[1], 1);
Pythia 0:a4131de4bddd 162 data[1] = data[1] & (0b00111111) | (cmd<<6);
Pythia 0:a4131de4bddd 163 _i2c->write(ACC_ADDRESS, data, 2);
Pythia 0:a4131de4bddd 164 }
Pythia 0:a4131de4bddd 165
Pythia 0:a4131de4bddd 166 void LSM303DLHC::ACtrl(ACC_HPCF cmd){
Pythia 0:a4131de4bddd 167 data[0] = CTRL_REG2_A;
Pythia 0:a4131de4bddd 168 _i2c->write(ACC_ADDRESS, data, 1);
Pythia 0:a4131de4bddd 169 _i2c->read(ACC_ADDRESS, &data[1], 1);
Pythia 0:a4131de4bddd 170 data[1] = data[1] & (0b11001111) | (cmd<<4);
Pythia 0:a4131de4bddd 171 _i2c->write(ACC_ADDRESS, data, 2);
Pythia 0:a4131de4bddd 172 }
Pythia 0:a4131de4bddd 173
Pythia 0:a4131de4bddd 174 void LSM303DLHC::ACtrl(ACC_FDS cmd){
Pythia 0:a4131de4bddd 175 data[0] = CTRL_REG2_A;
Pythia 0:a4131de4bddd 176 _i2c->write(ACC_ADDRESS, data, 1);
Pythia 0:a4131de4bddd 177 _i2c->read(ACC_ADDRESS, &data[1], 1);
Pythia 0:a4131de4bddd 178 data[1] = data[1] & (0b11110111) | (cmd<<3);
Pythia 0:a4131de4bddd 179 _i2c->write(ACC_ADDRESS, data, 2);
Pythia 0:a4131de4bddd 180 }
Pythia 0:a4131de4bddd 181
Pythia 0:a4131de4bddd 182 void LSM303DLHC::ACtrl(ACC_BDU cmd){
Pythia 0:a4131de4bddd 183 data[0] = CTRL_REG4_A;
Pythia 0:a4131de4bddd 184 _i2c->write(ACC_ADDRESS, data, 1);
Pythia 0:a4131de4bddd 185 _i2c->read(ACC_ADDRESS, &data[1], 1);
Pythia 0:a4131de4bddd 186 data[1] = data[1] & (0b01111111) | (cmd<<7);
Pythia 0:a4131de4bddd 187 _i2c->write(ACC_ADDRESS, data, 2);
Pythia 0:a4131de4bddd 188 }
Pythia 0:a4131de4bddd 189
Pythia 0:a4131de4bddd 190 static const int A_SCALE[4] = {1, 2, 4, 12};
Pythia 0:a4131de4bddd 191
Pythia 0:a4131de4bddd 192 void LSM303DLHC::ACtrl(ACC_FS cmd){
Pythia 0:a4131de4bddd 193 data[0] = CTRL_REG4_A;
Pythia 0:a4131de4bddd 194 _i2c->write(ACC_ADDRESS, data, 1);
Pythia 0:a4131de4bddd 195 _i2c->read(ACC_ADDRESS, &data[1], 1);
Pythia 0:a4131de4bddd 196 data[1] = data[1] & (0b11001111) | (cmd<<4);
Pythia 0:a4131de4bddd 197 _i2c->write(ACC_ADDRESS, data, 2);
Pythia 0:a4131de4bddd 198
Pythia 0:a4131de4bddd 199 acc_scale_multiplier = A_SCALE[cmd]/1000.;
Pythia 0:a4131de4bddd 200 }
Pythia 0:a4131de4bddd 201
Pythia 0:a4131de4bddd 202 void LSM303DLHC::ACtrl(ACC_HR cmd){
Pythia 0:a4131de4bddd 203 data[0] = CTRL_REG4_A;
Pythia 0:a4131de4bddd 204 _i2c->write(ACC_ADDRESS, data, 1);
Pythia 0:a4131de4bddd 205 _i2c->read(ACC_ADDRESS, &data[1], 1);
Pythia 0:a4131de4bddd 206 data[1] = data[1] & (0b11110111) | (cmd<<3);
Pythia 0:a4131de4bddd 207 _i2c->write(ACC_ADDRESS, data, 2);
Pythia 0:a4131de4bddd 208 }
Pythia 0:a4131de4bddd 209
Pythia 0:a4131de4bddd 210 void LSM303DLHC::TCtrl(TEMP_EN cmd){
Pythia 0:a4131de4bddd 211 data[0] = CRA_REG_M;
Pythia 0:a4131de4bddd 212 _i2c->write(MAG_ADDRESS, data, 1);
Pythia 0:a4131de4bddd 213 _i2c->read(MAG_ADDRESS, &data[1], 1);
Pythia 0:a4131de4bddd 214 data[1] = data[1] & (0b01111111) | (cmd<<7);
Pythia 0:a4131de4bddd 215 _i2c->write(MAG_ADDRESS, data, 2);
Pythia 0:a4131de4bddd 216 }
Pythia 0:a4131de4bddd 217
Pythia 0:a4131de4bddd 218 void LSM303DLHC::MCtrl(MAG_DR cmd){
Pythia 0:a4131de4bddd 219 data[0] = CRA_REG_M;
Pythia 0:a4131de4bddd 220 _i2c->write(MAG_ADDRESS, data, 1);
Pythia 0:a4131de4bddd 221 _i2c->read(MAG_ADDRESS, &data[1], 1);
Pythia 0:a4131de4bddd 222 data[1] = data[1] & (0b11100011) | (cmd<<2);
Pythia 0:a4131de4bddd 223 _i2c->write(MAG_ADDRESS, data, 2);
Pythia 0:a4131de4bddd 224 }
Pythia 0:a4131de4bddd 225
Pythia 0:a4131de4bddd 226 static const int M_SCALE_XY[8] = {1, 1100, 855, 670, 450, 400, 330, 230 };
Pythia 0:a4131de4bddd 227 static const int M_SCALE_Z[8] = {1, 980, 760, 600, 400, 355, 295, 205 };
Pythia 0:a4131de4bddd 228
Pythia 0:a4131de4bddd 229 void LSM303DLHC::MCtrl(MAG_GN cmd){
Pythia 0:a4131de4bddd 230 data[0] = CRB_REG_M;
Pythia 0:a4131de4bddd 231 _i2c->write(MAG_ADDRESS, data, 1);
Pythia 0:a4131de4bddd 232 _i2c->read(MAG_ADDRESS, &data[1], 1);
Pythia 0:a4131de4bddd 233 data[1] = data[1] & (0b00011111) | (cmd<<5);
Pythia 0:a4131de4bddd 234 _i2c->write(MAG_ADDRESS, data, 2);
Pythia 0:a4131de4bddd 235
Pythia 0:a4131de4bddd 236 mag_scale_x_multiplier = 1./M_SCALE_XY[cmd];
Pythia 0:a4131de4bddd 237 mag_scale_y_multiplier = 1./M_SCALE_XY[cmd];
Pythia 0:a4131de4bddd 238 mag_scale_z_multiplier = 1./M_SCALE_Z[cmd];
Pythia 0:a4131de4bddd 239 }
Pythia 0:a4131de4bddd 240
Pythia 0:a4131de4bddd 241 void LSM303DLHC::MCtrl(MAG_MD cmd){
Pythia 0:a4131de4bddd 242 data[0] = MR_REG_M;
Pythia 0:a4131de4bddd 243 _i2c->write(MAG_ADDRESS, data, 1);
Pythia 0:a4131de4bddd 244 _i2c->read(MAG_ADDRESS, &data[1], 1);
Pythia 0:a4131de4bddd 245 data[1] = data[1] & (0b11111100) | (cmd<<0);
Pythia 0:a4131de4bddd 246 _i2c->write(MAG_ADDRESS, data, 2);
Pythia 0:a4131de4bddd 247 }
Pythia 0:a4131de4bddd 248
Pythia 0:a4131de4bddd 249 void LSM303DLHC::WriteReg(int sad, char d[2]){
Pythia 0:a4131de4bddd 250 _i2c->write(sad, d, 2);
Pythia 0:a4131de4bddd 251 }
Pythia 0:a4131de4bddd 252
Pythia 0:a4131de4bddd 253 void LSM303DLHC::ACal(float offset[3], float scale[3]){
Pythia 0:a4131de4bddd 254 acc_offset[0] = offset[0];
Pythia 0:a4131de4bddd 255 acc_offset[1] = offset[1];
Pythia 0:a4131de4bddd 256 acc_offset[2] = offset[2];
Pythia 0:a4131de4bddd 257 acc_scale[0] = scale[0];
Pythia 0:a4131de4bddd 258 acc_scale[1] = scale[1];
Pythia 0:a4131de4bddd 259 acc_scale[2] = scale[2];
Pythia 0:a4131de4bddd 260 }
Pythia 0:a4131de4bddd 261
Pythia 0:a4131de4bddd 262 void LSM303DLHC::MCal(float offset[3], float scale[3]){
Pythia 0:a4131de4bddd 263 mag_offset[0] = offset[0];
Pythia 0:a4131de4bddd 264 mag_offset[1] = offset[1];
Pythia 0:a4131de4bddd 265 mag_offset[2] = offset[2];
Pythia 0:a4131de4bddd 266 mag_scale[0] = scale[0];
Pythia 0:a4131de4bddd 267 mag_scale[1] = scale[1];
Pythia 0:a4131de4bddd 268 mag_scale[2] = scale[2];
Pythia 0:a4131de4bddd 269 }
Pythia 0:a4131de4bddd 270
Pythia 0:a4131de4bddd 271 void LSM303DLHC::TCal(float offset[1], float scale[1]){
Pythia 0:a4131de4bddd 272 temp_offset[0] = offset[0];
Pythia 0:a4131de4bddd 273 temp_scale[0] = scale[0];
Pythia 0:a4131de4bddd 274 }