embox

1
/**
2
 * @file
3
 * @brief
4
 *
5
 * @date 21.04.2023
6
 * @author Andrew Bursian
7
 */
8

9
#include <unistd.h>
10
#include <util/log.h>
11
#include <endian.h>
12

13
#include "lsm6dsl.h"
14
#include "lsm6dsl_transfer.h"
15

16
#define LSM6DSL_WHO_AM_I	0xf
17
#define LSM6DSL_CTRL_REG(i)	(0x10 + i - 1)
18
#define LSM6DSL_STATUS_REG	0x1e
19
#define LSM6DSL_OUT_TEMP_L	0x20
20
#define LSM6DSL_OUT_TEMP_H	0x21
21
#define LSM6DSL_OUT_X_L_G	0x22
22
#define LSM6DSL_OUT_X_H_G	0x23
23
#define LSM6DSL_OUT_Y_L_G	0x24
24
#define LSM6DSL_OUT_Y_H_G	0x25
25
#define LSM6DSL_OUT_Z_L_G	0x26
26
#define LSM6DSL_OUT_Z_H_G	0x27
27
#define LSM6DSL_OUT_X_L_XL	0x28
28
#define LSM6DSL_OUT_X_H_XL	0x29
29
#define LSM6DSL_OUT_Y_L_XL	0x2a
30
#define LSM6DSL_OUT_Y_H_XL	0x2b
31
#define LSM6DSL_OUT_Z_L_XL	0x2c
32
#define LSM6DSL_OUT_Z_H_XL	0x2d
33
#define LSM6DSL_TIMESTAMP0_REG	0x40
34
#define LSM6DSL_TIMESTAMP1_REG	0x41
35
#define LSM6DSL_TIMESTAMP2_REG	0x42
36
#define LSM6DSL_WAKE_UP_DUR	0x5c
37

38
#define LSM6DSL_WHO_AM_I_VALUE 0x6A
39

40
#define LSM6DSL_TEMP_OFFSET		(25*256)
41
#define LSM6DSL_TIMEST_STEP		25
42

43
extern struct lsm6dsl_dev lsm6dsl_dev0;
44

45
void lsm6dsl_get_data(struct lsm6dsl_s *data) {
46
	struct lsm6dsl_dev *dev = &lsm6dsl_dev0;
47
	union {int16_t val; uint8_t b[2];}m;
48
	lsm6dsl_readb(dev, LSM6DSL_OUT_X_L_G, &(m.b[0]));
49
	lsm6dsl_readb(dev, LSM6DSL_OUT_X_H_G, &(m.b[1]));
50
	data->g_x = le16toh(m.val);
51
	lsm6dsl_readb(dev, LSM6DSL_OUT_Y_L_G, &(m.b[0]));
52
	lsm6dsl_readb(dev, LSM6DSL_OUT_Y_H_G, &(m.b[1]));
53
	data->g_y = le16toh(m.val);
54
	lsm6dsl_readb(dev, LSM6DSL_OUT_Z_L_G, &(m.b[0]));
55
	lsm6dsl_readb(dev, LSM6DSL_OUT_Z_H_G, &(m.b[1]));
56
	data->g_z = le16toh(m.val);
57
	lsm6dsl_readb(dev, LSM6DSL_OUT_X_L_XL, &(m.b[0]));
58
	lsm6dsl_readb(dev, LSM6DSL_OUT_X_H_XL, &(m.b[1]));
59
	data->xl_x = le16toh(m.val);
60
	lsm6dsl_readb(dev, LSM6DSL_OUT_Y_L_XL, &(m.b[0]));
61
	lsm6dsl_readb(dev, LSM6DSL_OUT_Y_H_XL, &(m.b[1]));
62
	data->xl_y = le16toh(m.val);
63
	lsm6dsl_readb(dev, LSM6DSL_OUT_Z_L_XL, &(m.b[0]));
64
	lsm6dsl_readb(dev, LSM6DSL_OUT_Z_H_XL, &(m.b[1]));
65
	data->xl_z = le16toh(m.val);
66
}
67

68
int16_t lsm6dsl_get_temp_x256(void) {
69
	struct lsm6dsl_dev *dev = &lsm6dsl_dev0;
70
	union {int16_t val; uint8_t b[2];}t;
71
	lsm6dsl_readb(dev, LSM6DSL_OUT_TEMP_L, &(t.b[0]));
72
	lsm6dsl_readb(dev, LSM6DSL_OUT_TEMP_H, &(t.b[1]));
73

74
	return le16toh(t.val) + LSM6DSL_TEMP_OFFSET;
75
}
76

77
int32_t lsm6dsl_get_timestamp_us(void) {
78
	struct lsm6dsl_dev *dev = &lsm6dsl_dev0;
79
	union {int32_t val; uint8_t b[4];}t;
80
	lsm6dsl_readb(dev, LSM6DSL_TIMESTAMP0_REG, &(t.b[0]));
81
	lsm6dsl_readb(dev, LSM6DSL_TIMESTAMP1_REG, &(t.b[1]));
82
	lsm6dsl_readb(dev, LSM6DSL_TIMESTAMP2_REG, &(t.b[2]));
83
	t.b[3] = 0; // TO DO: overflow catch!
84

85
	return le32toh(t.val)*LSM6DSL_TIMEST_STEP;
86
}
87

88
int lsm6dsl_init(void) {
89
	int i;
90
	uint8_t tmp;
91
	struct lsm6dsl_dev *dev = &lsm6dsl_dev0;
92

93
	if (lsm6dsl_hw_init(dev) < 0) {
94
		log_error("LSM6DSL hw init failed!");
95
		return -1;
96
	}
97

98
	if (lsm6dsl_readb(dev, LSM6DSL_WHO_AM_I, &tmp) < 0) {
99
		log_error("LSM6DSL readb failed!");
100
		return -1;
101
	}
102
	log_info("WHO_AM_I = 0x%02x", tmp);
103
	if (tmp != LSM6DSL_WHO_AM_I_VALUE) {
104
		log_error("LSM6DSL Device ID mismatch! %2x", tmp);
105
		return -1;
106
	}
107

108
	tmp = 0x20;	/* Accelerometer data rate 26 Hz (low power) */
109
	tmp |= 0<<2;	/* Accelerometer full-scale ±2 g */
110
	lsm6dsl_writeb(dev, LSM6DSL_CTRL_REG(1), tmp);
111

112
	tmp = 0x20;	/* Gyroscope data rate 26 Hz (low power) */
113
	tmp |= 0<<2;	/* Gyroscope full-scale 250 dps */
114
	lsm6dsl_writeb(dev, LSM6DSL_CTRL_REG(2), tmp);
115

116
	tmp = 0x40;	/* Block data update */
117
	tmp |= 0x4;	/* Register address autoincrement */
118
	lsm6dsl_writeb(dev, LSM6DSL_CTRL_REG(3), tmp);
119

120
//	tmp = 0x80;	/* DEN value stored in LSB of X-axis */
121
//	tmp = 0x40;	/* DEN value stored in LSB of Y-axis */
122
//	tmp = 0x20;	/* DEN value stored in LSB of Z-axis */
123
//	tmp |= 0x10;	/* DEN pin info stamped in the accelerometer axis selected by bits [7:5] */
124
//	lsm6dsl_writeb(dev, LSM6DSL_CTRL_REG(9), tmp);
125

126
	tmp = 0x10;	/* Reset timestamp counter */
127
	lsm6dsl_writeb(dev, LSM6DSL_WAKE_UP_DUR, tmp);
128
	tmp = 0xaa;	/* Reset timestamp counter */
129
	lsm6dsl_writeb(dev, LSM6DSL_TIMESTAMP2_REG, tmp);
130
	tmp = 0x20;	/* Enable timestamp counter */
131
	lsm6dsl_writeb(dev, LSM6DSL_CTRL_REG(10), tmp);
132

133
	for (i = 1; i <= 10; i++) {
134
		lsm6dsl_readb(dev, LSM6DSL_CTRL_REG(i), &tmp);
135
		log_info("CTRL_REG%d = 0x%02x", i, tmp);
136
	}
137
	lsm6dsl_readb(dev, LSM6DSL_STATUS_REG, &tmp);
138
	log_info("STATUS_REG = 0x%02x", tmp);
139
	return 0;
140
}
141