1 /*
2 * Copyright (c) 2018 Philémon Jaermann
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #define DT_DRV_COMPAT st_lsm303dlhc_magn
8
9 #include <zephyr/drivers/i2c.h>
10 #include <zephyr/init.h>
11 #include <zephyr/drivers/sensor.h>
12 #include <zephyr/logging/log.h>
13
14 LOG_MODULE_REGISTER(lsm303dlhc_magn, CONFIG_SENSOR_LOG_LEVEL);
15
16 #include "lsm303dlhc_magn.h"
17
lsm303dlhc_sample_fetch(const struct device * dev,enum sensor_channel chan)18 static int lsm303dlhc_sample_fetch(const struct device *dev,
19 enum sensor_channel chan)
20 {
21 const struct lsm303dlhc_magn_config *config = dev->config;
22 struct lsm303dlhc_magn_data *drv_data = dev->data;
23 uint8_t magn_buf[6];
24 uint8_t status;
25
26 /* Check data ready flag */
27 if (i2c_reg_read_byte_dt(&config->i2c, LSM303DLHC_SR_REG_M,
28 &status) < 0) {
29 LOG_ERR("Failed to read status register.");
30 return -EIO;
31 }
32
33 if (!(status & LSM303DLHC_MAGN_DRDY)) {
34 LOG_ERR("Sensor data not available.");
35 return -EIO;
36 }
37
38 if (i2c_burst_read_dt(&config->i2c, LSM303DLHC_REG_MAGN_X_LSB,
39 magn_buf, 6) < 0) {
40 LOG_ERR("Could not read magn axis data.");
41 return -EIO;
42 }
43
44 drv_data->magn_x = (magn_buf[0] << 8) | magn_buf[1];
45 drv_data->magn_y = (magn_buf[4] << 8) | magn_buf[5];
46 drv_data->magn_z = (magn_buf[2] << 8) | magn_buf[3];
47
48 return 0;
49 }
50
lsm303dlhc_convert_xy(struct sensor_value * val,int64_t raw_val)51 static void lsm303dlhc_convert_xy(struct sensor_value *val,
52 int64_t raw_val)
53 {
54 val->val1 = raw_val / LSM303DLHC_MAGN_LSB_GAUSS_XY;
55 val->val2 = (1000000 * raw_val / LSM303DLHC_MAGN_LSB_GAUSS_XY) % 1000000;
56 }
57
lsm303dlhc_convert_z(struct sensor_value * val,int64_t raw_val)58 static void lsm303dlhc_convert_z(struct sensor_value *val,
59 int64_t raw_val)
60 {
61 val->val1 = raw_val / LSM303DLHC_MAGN_LSB_GAUSS_Z;
62 val->val2 = (1000000 * raw_val / LSM303DLHC_MAGN_LSB_GAUSS_Z) % 1000000;
63 }
64
lsm303dlhc_channel_get(const struct device * dev,enum sensor_channel chan,struct sensor_value * val)65 static int lsm303dlhc_channel_get(const struct device *dev,
66 enum sensor_channel chan,
67 struct sensor_value *val)
68 {
69 struct lsm303dlhc_magn_data *drv_data = dev->data;
70
71 switch (chan) {
72 case SENSOR_CHAN_MAGN_X:
73 lsm303dlhc_convert_xy(val, drv_data->magn_x);
74 break;
75 case SENSOR_CHAN_MAGN_Y:
76 lsm303dlhc_convert_xy(val, drv_data->magn_y);
77 break;
78 case SENSOR_CHAN_MAGN_Z:
79 lsm303dlhc_convert_z(val, drv_data->magn_z);
80 break;
81 case SENSOR_CHAN_MAGN_XYZ:
82 lsm303dlhc_convert_xy(val, drv_data->magn_x);
83 lsm303dlhc_convert_xy(val + 1, drv_data->magn_y);
84 lsm303dlhc_convert_z(val + 2, drv_data->magn_z);
85 break;
86 default:
87 return -ENOTSUP;
88 }
89 return 0;
90 }
91
92 static DEVICE_API(sensor, lsm303dlhc_magn_driver_api) = {
93 .sample_fetch = lsm303dlhc_sample_fetch,
94 .channel_get = lsm303dlhc_channel_get,
95 };
96
lsm303dlhc_magn_init(const struct device * dev)97 static int lsm303dlhc_magn_init(const struct device *dev)
98 {
99 const struct lsm303dlhc_magn_config *config = dev->config;
100
101 if (!device_is_ready(config->i2c.bus)) {
102 LOG_ERR("I2C bus device not ready");
103 return -ENODEV;
104 }
105
106 /* Set magnetometer output data rate */
107 if (i2c_reg_write_byte_dt(&config->i2c, LSM303DLHC_CRA_REG_M,
108 LSM303DLHC_MAGN_ODR_BITS) < 0) {
109 LOG_ERR("Failed to configure chip.");
110 return -EIO;
111 }
112
113 /* Set magnetometer full scale range */
114 if (i2c_reg_write_byte_dt(&config->i2c, LSM303DLHC_CRB_REG_M,
115 LSM303DLHC_MAGN_FS_BITS) < 0) {
116 LOG_ERR("Failed to set magnetometer full scale range.");
117 return -EIO;
118 }
119
120 /* Continuous update */
121 if (i2c_reg_write_byte_dt(&config->i2c, LSM303DLHC_MR_REG_M,
122 LSM303DLHC_MAGN_CONT_UPDATE) < 0) {
123 LOG_ERR("Failed to enable continuous data update.");
124 return -EIO;
125 }
126 return 0;
127 }
128
129 #define LSM303DLHC_MAGN_DEFINE(inst) \
130 static struct lsm303dlhc_magn_data lsm303dlhc_magn_data_##inst; \
131 \
132 static const struct lsm303dlhc_magn_config lsm303dlhc_magn_config_##inst = { \
133 .i2c = I2C_DT_SPEC_INST_GET(inst), \
134 }; \
135 \
136 SENSOR_DEVICE_DT_INST_DEFINE(inst, lsm303dlhc_magn_init, NULL, \
137 &lsm303dlhc_magn_data_##inst, &lsm303dlhc_magn_config_##inst, \
138 POST_KERNEL, CONFIG_SENSOR_INIT_PRIORITY, \
139 &lsm303dlhc_magn_driver_api); \
140
141 DT_INST_FOREACH_STATUS_OKAY(LSM303DLHC_MAGN_DEFINE)
142
