Skip to content

Commit 21813e0

Browse files
aeratertaaeraterta
committed
Fix functionalities for lsm303
1 parent e8ed582 commit 21813e0

File tree

2 files changed

+197
-74
lines changed

2 files changed

+197
-74
lines changed

‎lsm303.c

+174-64
Original file line numberDiff line numberDiff line change
@@ -34,26 +34,33 @@
3434
* - 0 on success.
3535
* - Non-zero error code on failure.
3636
*/
37-
int8_t lsm303_setup(lsm303_dev **device, lsm303_init_param lsm303_params) {
37+
int8_t lsm303_setup(lsm303_dev *dev, lsm303_init_param lsm303_params) {
3838
int8_t ret = 0;
3939

40-
lsm303_dev *dev = (lsm303_dev *)k_malloc(sizeof(lsm303_dev));
41-
42-
if (dev == NULL) {
43-
return LSM303_STATUS_ALLOC_ERR;
44-
}
45-
4640
dev->acc_power_mode = lsm303_params.acc_power_mode;
4741
dev->acc_odr = lsm303_params.acc_odr;
4842
dev->acc_axes_config = lsm303_params.acc_axes_config;
4943
dev->acc_scale = lsm303_params.acc_scale;
44+
dev->acc_resolution = lsm303_params.acc_resolution;
45+
// dev->i2c0_dev = DEVICE_DT_GET(i2c0_master);
46+
dev->i2c0_dev = (struct device *)DEVICE_DT_GET(i2c0_master);
47+
48+
if (!device_is_ready(dev->i2c0_dev)) {
49+
printk("I2C bus is not ready!\n\r");
50+
return LSM303_STATUS_API_ERR;
51+
}
5052

5153
ret |= lsm303_set_power_mode(dev, dev->acc_power_mode);
5254
ret |= lsm303_acc_enable_axes(dev, dev->acc_axes_config);
5355
ret |= lsm303_acc_set_odr(dev, dev->acc_odr);
5456
ret |= lsm303_acc_set_scale(dev, dev->acc_scale);
57+
ret |= lsm303_acc_set_resolution(dev, dev->acc_resolution);
5558

56-
*device = dev;
59+
if (ret == LSM303_STATUS_SUCCESS) {
60+
dev->is_Setup = true;
61+
} else {
62+
dev->is_Setup = false;
63+
}
5764

5865
return ret;
5966
}
@@ -74,7 +81,6 @@ int8_t lsm303_setup(lsm303_dev **device, lsm303_init_param lsm303_params) {
7481
int8_t lsm303_set_power_mode(lsm303_dev *device,
7582
enum lsm303_acc_power_mode mode) {
7683
uint8_t val = 0x00;
77-
uint8_t data_buffer[2];
7884

7985
if (lsm303_i2c_read(device, ACC_I2C_ADDRESS, CTRL_REG1_A, &val) !=
8086
LSM303_STATUS_SUCCESS) {
@@ -84,16 +90,12 @@ int8_t lsm303_set_power_mode(lsm303_dev *device,
8490
if (mode == ACC_POWER_DOWN) {
8591
val &= ~0xF0;
8692
} else {
93+
val &= ~0x08;
8794
val = val | mode << ACC_POWER_MODE_MASK;
8895
}
96+
device->acc_power_mode = mode;
8997

90-
data_buffer[1] = CTRL_REG1_A;
91-
data_buffer[0] = val;
92-
93-
if (device->is_Setup) {
94-
device->acc_power_mode = mode;
95-
}
96-
98+
uint8_t data_buffer[] = {CTRL_REG1_A, val};
9799
return lsm303_i2c_write(device, ACC_I2C_ADDRESS, data_buffer);
98100
}
99101

@@ -113,7 +115,6 @@ int8_t lsm303_set_power_mode(lsm303_dev *device,
113115
*/
114116
int8_t lsm303_acc_enable_axes(lsm303_dev *device, lsm303_acc_axes_config axes) {
115117
uint8_t val = 0x00;
116-
uint8_t data_buffer[2];
117118

118119
if (lsm303_i2c_read(device, ACC_I2C_ADDRESS, CTRL_REG1_A, &val) !=
119120
LSM303_STATUS_SUCCESS) {
@@ -122,18 +123,12 @@ int8_t lsm303_acc_enable_axes(lsm303_dev *device, lsm303_acc_axes_config axes) {
122123

123124
val &= ~0x07;
124125
val = val | axes.acc_axes << ACC_AXES_MASK;
126+
axes.enable.z = (val & (1 << 2)) >> 2;
127+
axes.enable.y = (val & (1 << 1)) >> 1;
128+
axes.enable.x = (val & 1);
129+
device->acc_axes_config = axes;
125130

126-
data_buffer[1] = CTRL_REG1_A;
127-
data_buffer[0] = val;
128-
129-
axes.enable.z = val & (1 << 2);
130-
axes.enable.y = val & (1 << 1);
131-
axes.enable.x = val & 1;
132-
133-
if (device->is_Setup) {
134-
device->acc_axes_config = axes;
135-
}
136-
131+
uint8_t data_buffer[] = {CTRL_REG1_A, val};
137132
return lsm303_i2c_write(device, ACC_I2C_ADDRESS, data_buffer);
138133
}
139134

@@ -152,7 +147,6 @@ int8_t lsm303_acc_enable_axes(lsm303_dev *device, lsm303_acc_axes_config axes) {
152147
*/
153148
int8_t lsm303_acc_set_odr(lsm303_dev *device, enum lsm303_acc_odr odr) {
154149
uint8_t val = 0x00;
155-
uint8_t data_buffer[2];
156150

157151
if (lsm303_i2c_read(device, ACC_I2C_ADDRESS, CTRL_REG1_A, &val) !=
158152
LSM303_STATUS_SUCCESS) {
@@ -161,14 +155,9 @@ int8_t lsm303_acc_set_odr(lsm303_dev *device, enum lsm303_acc_odr odr) {
161155

162156
val &= ~0xF0;
163157
val = val | odr << ACC_ODR_MASK;
158+
device->acc_odr = odr;
164159

165-
data_buffer[1] = CTRL_REG1_A;
166-
data_buffer[0] = val;
167-
168-
if (device->is_Setup) {
169-
device->acc_odr = odr;
170-
}
171-
160+
uint8_t data_buffer[] = {CTRL_REG1_A, val};
172161
return lsm303_i2c_write(device, ACC_I2C_ADDRESS, data_buffer);
173162
}
174163

@@ -188,23 +177,47 @@ int8_t lsm303_acc_set_odr(lsm303_dev *device, enum lsm303_acc_odr odr) {
188177
int8_t lsm303_acc_set_scale(lsm303_dev *device,
189178
enum lsm303_acc_full_scale scale) {
190179
uint8_t val = 0x00;
191-
uint8_t data_buffer[2];
192180

193-
if (lsm303_i2c_read(device, ACC_I2C_ADDRESS, CTRL_REG1_A, &val) !=
181+
if (lsm303_i2c_read(device, ACC_I2C_ADDRESS, CTRL_REG4_A, &val) !=
194182
LSM303_STATUS_SUCCESS) {
195183
return LSM303_STATUS_API_ERR;
196184
}
197185

198-
val &= ~0x18;
186+
val &= ~0x30;
199187
val = val | scale << ACC_SCALE_MASK;
188+
device->acc_scale = scale;
200189

201-
data_buffer[1] = CTRL_REG4_A;
202-
data_buffer[0] = val;
190+
uint8_t data_buffer[] = {CTRL_REG4_A, val};
191+
return lsm303_i2c_write(device, ACC_I2C_ADDRESS, data_buffer);
192+
}
193+
194+
/**
195+
* @brief Configures the accelerometer resolution for the LSM303 device.
196+
*
197+
* Sets the resolution of the accelerometer based on the specified
198+
* resolution parameter.
199+
*
200+
* @param device Pointer to the LSM303 device structure.
201+
* @param resolution Resolution setting for the accelerometer.
202+
*
203+
* @return
204+
* - 0 on success.
205+
* - Non-zero error code on failure.
206+
*/
207+
int8_t lsm303_acc_set_resolution(lsm303_dev *device,
208+
enum lsm303_acc_resolution resolution) {
209+
uint8_t val = 0x00;
203210

204-
if (device->is_Setup) {
205-
device->acc_scale = scale;
211+
if (lsm303_i2c_read(device, ACC_I2C_ADDRESS, CTRL_REG4_A, &val) !=
212+
LSM303_STATUS_SUCCESS) {
213+
return LSM303_STATUS_API_ERR;
206214
}
207215

216+
val &= ~0x08;
217+
val = val | resolution << ACC_RESOLUTION_MASK;
218+
device->acc_resolution = resolution;
219+
220+
uint8_t data_buffer[] = {CTRL_REG4_A, val};
208221
return lsm303_i2c_write(device, ACC_I2C_ADDRESS, data_buffer);
209222
}
210223

@@ -248,22 +261,29 @@ int8_t lsm303_data_ready(lsm303_dev *device) {
248261
* - 0 on success.
249262
* - Non-zero error code on failure.
250263
*/
251-
int8_t lsm303_get_x_data(lsm303_dev *device, lsm303_axes_data *accel_data) {
252-
uint8_t val = 0x00;
264+
int8_t lsm303_get_x_raw_data(lsm303_dev *device, lsm303_axes_data *accel_data) {
265+
uint8_t val_l = 0x00;
266+
uint8_t val_h = 0x00;
253267

254-
if (lsm303_i2c_read(device, ACC_I2C_ADDRESS, OUT_X_H_A, &val) !=
268+
if (lsm303_i2c_read(device, ACC_I2C_ADDRESS, OUT_X_H_A, &val_h) !=
255269
LSM303_STATUS_SUCCESS) {
256270
return LSM303_STATUS_API_ERR;
257271
}
258272

259-
accel_data->x = (int16_t)(val << 8);
260-
261-
if (lsm303_i2c_read(device, ACC_I2C_ADDRESS, OUT_X_L_A, &val) !=
273+
if (lsm303_i2c_read(device, ACC_I2C_ADDRESS, OUT_X_L_A, &val_l) !=
262274
LSM303_STATUS_SUCCESS) {
263275
return LSM303_STATUS_API_ERR;
264276
}
265277

266-
accel_data->x |= (int16_t)val;
278+
if (device->acc_resolution == ACC_RESOLUTION_LOW) {
279+
accel_data->x = (int16_t)(val_l | (val_h << 8)) >> 4;
280+
} else {
281+
if (device->acc_power_mode == ACC_NORMAL) {
282+
accel_data->x = (int16_t)(val_l | (val_h << 8));
283+
} else {
284+
accel_data->x = (int16_t)(val_l | (val_h << 8)) >> 2;
285+
}
286+
}
267287

268288
return LSM303_STATUS_SUCCESS;
269289
}
@@ -282,22 +302,29 @@ int8_t lsm303_get_x_data(lsm303_dev *device, lsm303_axes_data *accel_data) {
282302
* - 0 on success.
283303
* - Non-zero error code on failure.
284304
*/
285-
int8_t lsm303_get_y_data(lsm303_dev *device, lsm303_axes_data *accel_data) {
286-
uint8_t val = 0x00;
305+
int8_t lsm303_get_y_raw_data(lsm303_dev *device, lsm303_axes_data *accel_data) {
306+
uint8_t val_l = 0x00;
307+
uint8_t val_h = 0x00;
287308

288-
if (lsm303_i2c_read(device, ACC_I2C_ADDRESS, OUT_Y_H_A, &val) !=
309+
if (lsm303_i2c_read(device, ACC_I2C_ADDRESS, OUT_Y_H_A, &val_h) !=
289310
LSM303_STATUS_SUCCESS) {
290311
return LSM303_STATUS_API_ERR;
291312
}
292313

293-
accel_data->y = (int16_t)(val << 8);
294-
295-
if (lsm303_i2c_read(device, ACC_I2C_ADDRESS, OUT_Y_L_A, &val) !=
314+
if (lsm303_i2c_read(device, ACC_I2C_ADDRESS, OUT_Y_L_A, &val_l) !=
296315
LSM303_STATUS_SUCCESS) {
297316
return LSM303_STATUS_API_ERR;
298317
}
299318

300-
accel_data->y |= (int16_t)val;
319+
if (device->acc_resolution == ACC_RESOLUTION_LOW) {
320+
accel_data->y = (int16_t)(val_l | (val_h << 8)) >> 4;
321+
} else {
322+
if (device->acc_power_mode == ACC_NORMAL) {
323+
accel_data->y = (int16_t)(val_l | (val_h << 8));
324+
} else {
325+
accel_data->y = (int16_t)(val_l | (val_h << 8)) >> 2;
326+
}
327+
}
301328

302329
return LSM303_STATUS_SUCCESS;
303330
}
@@ -316,26 +343,109 @@ int8_t lsm303_get_y_data(lsm303_dev *device, lsm303_axes_data *accel_data) {
316343
* - 0 on success.
317344
* - Non-zero error code on failure.
318345
*/
319-
int8_t lsm303_get_z_data(lsm303_dev *device, lsm303_axes_data *accel_data) {
320-
uint8_t val = 0x00;
346+
int8_t lsm303_get_z_raw_data(lsm303_dev *device, lsm303_axes_data *accel_data) {
347+
uint8_t val_l = 0x00;
348+
uint8_t val_h = 0x00;
321349

322-
if (lsm303_i2c_read(device, ACC_I2C_ADDRESS, OUT_Z_H_A, &val) !=
350+
if (lsm303_i2c_read(device, ACC_I2C_ADDRESS, OUT_Z_H_A, &val_h) !=
323351
LSM303_STATUS_SUCCESS) {
324352
return LSM303_STATUS_API_ERR;
325353
}
326354

327-
accel_data->z = (int16_t)(val << 8);
328-
329-
if (lsm303_i2c_read(device, ACC_I2C_ADDRESS, OUT_Z_L_A, &val) !=
355+
if (lsm303_i2c_read(device, ACC_I2C_ADDRESS, OUT_Z_L_A, &val_l) !=
330356
LSM303_STATUS_SUCCESS) {
331357
return LSM303_STATUS_API_ERR;
332358
}
333359

334-
accel_data->z |= (int16_t)val;
360+
if (device->acc_resolution == ACC_RESOLUTION_LOW) {
361+
accel_data->z = (int16_t)(val_l | (val_h << 8)) >> 4;
362+
} else {
363+
if (device->acc_power_mode == ACC_NORMAL) {
364+
accel_data->z = (int16_t)(val_l | (val_h << 8));
365+
} else {
366+
accel_data->z = (int16_t)(val_l | (val_h << 8)) >> 2;
367+
}
368+
}
335369

336370
return LSM303_STATUS_SUCCESS;
337371
}
338372

373+
float convert_raw_to_g(lsm303_dev *device, int16_t raw_value) {
374+
float sensitivity = 0.0;
375+
376+
if (device->acc_resolution == ACC_RESOLUTION_LOW) {
377+
if (device->acc_power_mode == ACC_NORMAL) {
378+
// NORMAL MODE / LOW RESOLUTION
379+
switch (device->acc_scale) {
380+
case ACC_SCALE_2G:
381+
sensitivity = 1.0;
382+
break;
383+
case ACC_SCALE_4G:
384+
sensitivity = 2.0;
385+
break;
386+
case ACC_SCALE_8G:
387+
sensitivity = 4.0;
388+
break;
389+
case ACC_SCALE_16G:
390+
sensitivity = 12.0;
391+
break;
392+
}
393+
} else {
394+
// LOW POWER MODE / LOW RESOLUTION
395+
switch (device->acc_scale) {
396+
case ACC_SCALE_2G:
397+
sensitivity = 1.0;
398+
break;
399+
case ACC_SCALE_4G:
400+
sensitivity = 2.0;
401+
break;
402+
case ACC_SCALE_8G:
403+
sensitivity = 4.0;
404+
break;
405+
case ACC_SCALE_16G:
406+
sensitivity = 8.0;
407+
break;
408+
}
409+
}
410+
} else {
411+
if (device->acc_power_mode == ACC_NORMAL) {
412+
// NORMAL MODE / HIGH RESOLUTION
413+
switch (device->acc_scale) {
414+
case ACC_SCALE_2G:
415+
sensitivity = 0.0625;
416+
break;
417+
case ACC_SCALE_4G:
418+
sensitivity = 0.125;
419+
break;
420+
case ACC_SCALE_8G:
421+
sensitivity = 0.25;
422+
break;
423+
case ACC_SCALE_16G:
424+
sensitivity = 0.5;
425+
break;
426+
}
427+
} else {
428+
// LOW POWER MODE / LOW RESOLUTION
429+
switch (device->acc_scale) {
430+
case ACC_SCALE_2G:
431+
sensitivity = 0.25;
432+
break;
433+
case ACC_SCALE_4G:
434+
sensitivity = 0.5;
435+
break;
436+
case ACC_SCALE_8G:
437+
sensitivity = 1.0;
438+
break;
439+
case ACC_SCALE_16G:
440+
sensitivity = 2.0;
441+
break;
442+
}
443+
}
444+
}
445+
446+
return (float)raw_value * sensitivity / 1000.0;
447+
}
448+
339449
/**
340450
* @brief Reads a register value from the LSM303 device via I2C.
341451
*

0 commit comments

Comments
 (0)