ESPHome 2026.11.0b6
Loading...
Searching...
No Matches
lsm6ds.cpp
Go to the documentation of this file.
1#include "lsm6ds.h"
2#include "esphome/core/log.h"
3#include "esphome/core/hal.h"
6
7namespace esphome::lsm6ds {
8
9ESPHOME_LOG_TAG(TAG, "lsm6ds");
10
11// Indexed by the accel range field, which is not in ascending order (datasheet Table 3)
12PROGMEM_STRING_TABLE(AccelRangeStrings, "±2g", "±16g", "±4g", "±8g");
13
14static const LogString *gyro_range_to_str(LSM6DSGyroRange range) {
15 switch (range) {
17 return LOG_STR("±125dps");
19 return LOG_STR("±250dps");
21 return LOG_STR("±500dps");
23 return LOG_STR("±1000dps");
25 return LOG_STR("±2000dps");
26 default:
27 return LOG_STR("unknown");
28 }
29}
30
31const LogString *chip_name_from_who_am_i(uint8_t who_am_i) {
32 switch (who_am_i) {
33 case LSM6DS_WHO_AM_I_LSM6DS3:
34 return LOG_STR("LSM6DS3");
35 case LSM6DS_WHO_AM_I_LSM6DS3TR_C:
36 return LOG_STR("LSM6DS3TR-C");
37 case LSM6DS_WHO_AM_I_LSM6DSO:
38 return LOG_STR("LSM6DSO");
39 default:
40 return nullptr;
41 }
42}
43
44uint16_t temperature_sensitivity_from_who_am_i(uint8_t who_am_i) {
45 return who_am_i == LSM6DS_WHO_AM_I_LSM6DS3 ? 16 : 256;
46}
47
49 MotionComponent::setup();
50 uint8_t who_am_i = 0;
51 if (this->read_register(LSM6DS_REG_WHO_AM_I, &who_am_i, 1) != i2c::ERROR_OK) {
52 ESP_LOGE(TAG, "Failed to read WHO_AM_I — check wiring and I2C address");
53 this->mark_failed();
54 return;
55 }
56 const LogString *chip_name = chip_name_from_who_am_i(who_am_i);
57 if (chip_name == nullptr) {
58 ESP_LOGE(TAG, "Unknown WHO_AM_I: 0x%02X", who_am_i);
59 this->mark_failed(LOG_STR("Unknown WHO_AM_I value"));
60 return;
61 }
62 ESP_LOGD(TAG, "Found %s (WHO_AM_I = 0x%02X)", LOG_STR_ARG(chip_name), who_am_i);
63 this->chip_name_ = chip_name;
65
66 // 2. Software reset — clears all registers to defaults
67 if (this->write_register(LSM6DS_REG_CTRL3_C, &CTRL3_C_SW_RESET, 1) != i2c::ERROR_OK) {
68 this->mark_failed(LOG_STR("Software reset failed"));
69 return;
70 }
71 // Datasheet: reset bit self-clears after boot (typ. 50 µs);
72 delay(2);
73
74 // 3. Enable auto-increment and block data update (BDU).
75 // BDU prevents reading a high-byte from one sample and a low-byte from the next.
76 // IF_INC is set by default after reset but we set it explicitly for clarity.
77 uint8_t ctrl3 = CTRL3_C_IF_INC | CTRL3_C_BDU;
78 if (this->write_register(LSM6DS_REG_CTRL3_C, &ctrl3, 1) != i2c::ERROR_OK) {
79 this->mark_failed(LOG_STR("Config failed"));
80 return;
81 }
82
83 // 4. Configure accelerometer: ODR in bits[7:4], FS in bits[3:2]
84 // Anti-aliasing filter bandwidth is left at power-on default (bits[1:0] = 00 = ODR/2).
85 uint8_t ctrl1_xl = (uint8_t) (this->accel_odr_ << 4) | (uint8_t) (this->accel_range_ << 2);
86 if (this->write_register(LSM6DS_REG_CTRL1_XL, &ctrl1_xl, 1) != i2c::ERROR_OK) {
87 this->mark_failed(LOG_STR("Failed to configure accelerometer"));
88 return;
89 }
90
91 // 5. Configure gyroscope: ODR in bits[7:4], FS_G + FS_125 in bits[3:0]
92 // For ±125 dps: FS_G[2:1]=00 and FS_125(bit1)=1, so gyro_range_ encodes the full nibble.
93 uint8_t ctrl2_g = (uint8_t) (this->gyro_odr_ << 4) | (uint8_t) (this->gyro_range_);
94 if (this->write_register(LSM6DS_REG_CTRL2_G, &ctrl2_g, 1) != i2c::ERROR_OK) {
95 this->mark_failed(LOG_STR("Failed to configure gyroscope"));
96 return;
97 }
98
99 // 6. Ensure accelerometer is in high-performance mode (CTRL6_C bit 4 = XL_HM_MODE = 0)
100 // and gyroscope is in high-performance mode (CTRL7_G bit 7 = G_HM_MODE = 0).
101 // Both default to 0 (high-performance) after reset, but write explicitly.
102 uint8_t zero = 0x00;
103 if (this->write_register(LSM6DS_REG_CTRL6_C, &zero, 1) != i2c::ERROR_OK) {
104 this->mark_failed();
105 return;
106 }
107 if (this->write_register(LSM6DS_REG_CTRL7_G, &zero, 1) != i2c::ERROR_OK) {
108 this->mark_failed();
109 return;
110 }
111}
112
114 ESP_LOGCONFIG(TAG,
115 "LSM6DS IMU:\n"
116 " Chip type: %s\n",
117 LOG_STR_ARG(this->chip_name_ != nullptr ? this->chip_name_ : LOG_STR("Unknown")));
118 LOG_I2C_DEVICE(this);
119 LOG_UPDATE_INTERVAL(this);
120
121 auto accel_odr = this->accel_odr_ == 0 ? 0 : 13 * (1 << (this->accel_odr_ - 1));
122 auto gyro_odr = this->gyro_odr_ == 0 ? 0 : 13 * (1 << (this->gyro_odr_ - 1));
123 ESP_LOGCONFIG(TAG,
124 " Accel range : %s\n"
125 " Accel data rate : %dHz\n"
126 " Gyro range : %s\n"
127 " Gyro data rate : %dHz",
128 LOG_STR_ARG(AccelRangeStrings::get_log_str(this->accel_range_, AccelRangeStrings::LAST_INDEX)),
129 accel_odr, LOG_STR_ARG(gyro_range_to_str(this->gyro_range_)), gyro_odr);
130}
131
132// update_data()
133// Called by MotionComponent::update() on each polling interval.
134// Reads gyro XYZ and accel XYZ in a single 12-byte burst (registers 0x22–0x2D).
135// Values are in g (accel) and °/s (gyro) — MotionComponent handles axis mapping
136// and sensor publishing.
137
139 if (this->is_failed())
140 return false;
141
142 // Single burst: gyro X/Y/Z (0x22–0x27) then accel X/Y/Z (0x28–0x2D)
143 uint8_t raw[LSM6DS_BURST_LEN];
144 if (!this->read_bytes(LSM6DS_REG_OUTX_L_G, raw, LSM6DS_BURST_LEN)) {
145 this->status_set_error(LOG_STR("Failed to read IMU data"));
146 return false;
147 }
148 this->status_clear_error();
149
150 // Gyroscope
151 // Sensitivity (mdps/LSB) from datasheet Table 3.
152 // Multiply by 1e-3 to convert mdps → dps (°/s).
153 static constexpr float GYRO_SCALE[] = {
154 8.75e-3f, // 0x00 — ±250 dps
155 8.75e-3f, // 0x01 — unused (maps to 250 as fallback)
156 4.375e-3f, // 0x02 — ±125 dps (FS_125 bit set)
157 8.75e-3f, // 0x03 — unused
158 17.50e-3f, // 0x04 — ±500 dps
159 17.50e-3f, // 0x05 — unused
160 8.75e-3f, // 0x06 — unused
161 8.75e-3f, // 0x07 — unused
162 35.0e-3f, // 0x08 — ±1000 dps
163 35.0e-3f, // 0x09 — unused
164 17.50e-3f, // 0x0A — unused
165 17.50e-3f, // 0x0B — unused
166 70.0e-3f, // 0x0C — ±2000 dps
167 };
168 float gyro_scale = GYRO_SCALE[this->gyro_range_];
169
170 data.angular_rate[motion::X_AXIS] = encode_value_le<int16_t>(raw) * gyro_scale;
171 data.angular_rate[motion::Y_AXIS] = encode_value_le<int16_t>(&raw[2]) * gyro_scale;
172 data.angular_rate[motion::Z_AXIS] = encode_value_le<int16_t>(&raw[4]) * gyro_scale;
173
174 // Accelerometer
175 // Sensitivity (mg/LSB) from datasheet Table 3.
176 // Multiply by 1e-3 to convert mg → g.
177 // Note: FS_XL register values are non-monotonic (0=2g, 1=16g, 2=4g, 3=8g).
178 static constexpr float ACCEL_SCALE[] = {
179 0.061e-3f, // 0x00 — ±2g
180 0.488e-3f, // 0x01 — ±16g
181 0.122e-3f, // 0x02 — ±4g
182 0.244e-3f, // 0x03 — ±8g
183 };
184 float accel_scale = ACCEL_SCALE[this->accel_range_];
185
186 data.acceleration[motion::X_AXIS] = encode_value_le<int16_t>(&raw[LSM6DS_ACCEL_OFFSET]) * accel_scale;
187 data.acceleration[motion::Y_AXIS] = encode_value_le<int16_t>(&raw[LSM6DS_ACCEL_OFFSET + 2]) * accel_scale;
188 data.acceleration[motion::Z_AXIS] = encode_value_le<int16_t>(&raw[LSM6DS_ACCEL_OFFSET + 4]) * accel_scale;
189
190 // Temperature (lazy — only read if a listener is registered)
191 // Kept as a separate 2-byte read to avoid extending the burst to 14 bytes when
192 // temperature is not needed.
193 // Formula: T(°C) = raw / sensitivity + 25.0, sensitivity in LSB/°C per chip (0 LSB at 25 °C)
194 if (!this->temperature_callback_.empty()) {
195 uint8_t raw_t[2];
196 if (this->read_bytes(LSM6DS_REG_OUT_TEMP_L, raw_t, 2)) {
197 int16_t temp_raw = encode_value_le<int16_t>(raw_t);
198 float temperature = temp_raw / static_cast<float>(this->temperature_sensitivity_) + 25.0f;
199 this->temperature_callback_.call(temperature);
200 }
201 }
202
203 return true;
204}
205
206} // namespace esphome::lsm6ds
uint8_t raw[35]
Definition bl0939.h:0
void mark_failed()
Mark this component as failed.
bool is_failed() const
Definition component.h:287
void status_clear_error()
Definition component.h:315
ErrorCode write_register(uint8_t a_register, const uint8_t *data, size_t len) const
writes an array of bytes to a specific register in the I²C device
Definition i2c.cpp:54
ErrorCode read_register(uint8_t a_register, uint8_t *data, size_t len)
reads an array of bytes from a specific register in the I²C device
Definition i2c.cpp:45
bool read_bytes(uint8_t a_register, uint8_t *data, uint8_t len)
Compat APIs All methods below have been added for compatibility reasons.
Definition i2c.h:249
LazyCallbackManager< void(float)> temperature_callback_
Definition lsm6ds.h:118
bool update_data(motion::MotionData &data) override
Definition lsm6ds.cpp:138
LSM6DSGyroRange gyro_range_
Definition lsm6ds.h:114
LSM6DSAccelRange accel_range_
Definition lsm6ds.h:112
const LogString * chip_name_
Definition lsm6ds.h:109
Range range
Definition msa3xx.h:0
@ ERROR_OK
No error found during execution of method.
Definition i2c_bus.h:15
uint16_t temperature_sensitivity_from_who_am_i(uint8_t who_am_i)
Temperature sensitivity in LSB/°C for a supported WHO_AM_I value, from the chip's datasheet.
Definition lsm6ds.cpp:44
ESPHOME_LOG_TAG(TAG, "lsm6ds")
const LogString * chip_name_from_who_am_i(uint8_t who_am_i)
Chip name for a WHO_AM_I value, or nullptr when the chip is not supported.
Definition lsm6ds.cpp:31
@ LSM6DS_GYRO_RANGE_250
Definition lsm6ds.h:67
@ LSM6DS_GYRO_RANGE_125
Definition lsm6ds.h:66
@ LSM6DS_GYRO_RANGE_2000
Definition lsm6ds.h:70
@ LSM6DS_GYRO_RANGE_1000
Definition lsm6ds.h:69
@ LSM6DS_GYRO_RANGE_500
Definition lsm6ds.h:68
PROGMEM_STRING_TABLE(AccelRangeStrings, "±2g", "±16g", "±4g", "±8g")
constexpr T encode_value_le(const uint8_t *bytes)
Encode a value from its constituent bytes (from least to most significant) in an array with length si...
Definition helpers.h:1084
void HOT delay(uint32_t ms)
Definition hal.cpp:85
uint16_t temperature
Definition sun_gtil2.cpp:12