ESPHome  2022.12.8
mpu6050.cpp
Go to the documentation of this file.
1 #include "mpu6050.h"
2 #include "esphome/core/log.h"
3 
4 namespace esphome {
5 namespace mpu6050 {
6 
7 static const char *const TAG = "mpu6050";
8 
9 const uint8_t MPU6050_REGISTER_WHO_AM_I = 0x75;
11 const uint8_t MPU6050_REGISTER_GYRO_CONFIG = 0x1B;
12 const uint8_t MPU6050_REGISTER_ACCEL_CONFIG = 0x1C;
13 const uint8_t MPU6050_REGISTER_ACCEL_XOUT_H = 0x3B;
14 const uint8_t MPU6050_CLOCK_SOURCE_X_GYRO = 0b001;
15 const uint8_t MPU6050_SCALE_2000_DPS = 0b11;
16 const float MPU6050_SCALE_DPS_PER_DIGIT_2000 = 0.060975f;
17 const uint8_t MPU6050_RANGE_2G = 0b00;
18 const float MPU6050_RANGE_PER_DIGIT_2G = 0.000061f;
19 const uint8_t MPU6050_BIT_SLEEP_ENABLED = 6;
21 const float GRAVITY_EARTH = 9.80665f;
22 
24  ESP_LOGCONFIG(TAG, "Setting up MPU6050...");
25  uint8_t who_am_i;
26  if (!this->read_byte(MPU6050_REGISTER_WHO_AM_I, &who_am_i) || (who_am_i != 0x68 && who_am_i != 0x98)) {
27  this->mark_failed();
28  return;
29  }
30 
31  ESP_LOGV(TAG, " Setting up Power Management...");
32  // Setup power management
33  uint8_t power_management;
34  if (!this->read_byte(MPU6050_REGISTER_POWER_MANAGEMENT_1, &power_management)) {
35  this->mark_failed();
36  return;
37  }
38  ESP_LOGV(TAG, " Input power_management: 0b" BYTE_TO_BINARY_PATTERN, BYTE_TO_BINARY(power_management));
39  // Set clock source - X-Gyro
40  power_management &= 0b11111000;
41  power_management |= MPU6050_CLOCK_SOURCE_X_GYRO;
42  // Disable sleep
43  power_management &= ~(1 << MPU6050_BIT_SLEEP_ENABLED);
44  // Enable temperature
45  power_management &= ~(1 << MPU6050_BIT_TEMPERATURE_DISABLED);
46  ESP_LOGV(TAG, " Output power_management: 0b" BYTE_TO_BINARY_PATTERN, BYTE_TO_BINARY(power_management));
47  if (!this->write_byte(MPU6050_REGISTER_POWER_MANAGEMENT_1, power_management)) {
48  this->mark_failed();
49  return;
50  }
51 
52  ESP_LOGV(TAG, " Setting up Gyro Config...");
53  // Set scale - 2000DPS
54  uint8_t gyro_config;
55  if (!this->read_byte(MPU6050_REGISTER_GYRO_CONFIG, &gyro_config)) {
56  this->mark_failed();
57  return;
58  }
59  ESP_LOGV(TAG, " Input gyro_config: 0b" BYTE_TO_BINARY_PATTERN, BYTE_TO_BINARY(gyro_config));
60  gyro_config &= 0b11100111;
61  gyro_config |= MPU6050_SCALE_2000_DPS << 3;
62  ESP_LOGV(TAG, " Output gyro_config: 0b" BYTE_TO_BINARY_PATTERN, BYTE_TO_BINARY(gyro_config));
63  if (!this->write_byte(MPU6050_REGISTER_GYRO_CONFIG, gyro_config)) {
64  this->mark_failed();
65  return;
66  }
67 
68  ESP_LOGV(TAG, " Setting up Accel Config...");
69  // Set range - 2G
70  uint8_t accel_config;
71  if (!this->read_byte(MPU6050_REGISTER_ACCEL_CONFIG, &accel_config)) {
72  this->mark_failed();
73  return;
74  }
75  ESP_LOGV(TAG, " Input accel_config: 0b" BYTE_TO_BINARY_PATTERN, BYTE_TO_BINARY(accel_config));
76  accel_config &= 0b11100111;
77  accel_config |= (MPU6050_RANGE_2G << 3);
78  ESP_LOGV(TAG, " Output accel_config: 0b" BYTE_TO_BINARY_PATTERN, BYTE_TO_BINARY(accel_config));
79  if (!this->write_byte(MPU6050_REGISTER_GYRO_CONFIG, gyro_config)) {
80  this->mark_failed();
81  return;
82  }
83 }
85  ESP_LOGCONFIG(TAG, "MPU6050:");
86  LOG_I2C_DEVICE(this);
87  if (this->is_failed()) {
88  ESP_LOGE(TAG, "Communication with MPU6050 failed!");
89  }
90  LOG_UPDATE_INTERVAL(this);
91  LOG_SENSOR(" ", "Acceleration X", this->accel_x_sensor_);
92  LOG_SENSOR(" ", "Acceleration Y", this->accel_y_sensor_);
93  LOG_SENSOR(" ", "Acceleration Z", this->accel_z_sensor_);
94  LOG_SENSOR(" ", "Gyro X", this->gyro_x_sensor_);
95  LOG_SENSOR(" ", "Gyro Y", this->gyro_y_sensor_);
96  LOG_SENSOR(" ", "Gyro Z", this->gyro_z_sensor_);
97  LOG_SENSOR(" ", "Temperature", this->temperature_sensor_);
98 }
99 
101  ESP_LOGV(TAG, " Updating MPU6050...");
102  uint16_t raw_data[7];
103  if (!this->read_bytes_16(MPU6050_REGISTER_ACCEL_XOUT_H, raw_data, 7)) {
104  this->status_set_warning();
105  return;
106  }
107  auto *data = reinterpret_cast<int16_t *>(raw_data);
108 
109  float accel_x = data[0] * MPU6050_RANGE_PER_DIGIT_2G * GRAVITY_EARTH;
110  float accel_y = data[1] * MPU6050_RANGE_PER_DIGIT_2G * GRAVITY_EARTH;
111  float accel_z = data[2] * MPU6050_RANGE_PER_DIGIT_2G * GRAVITY_EARTH;
112 
113  float temperature = data[3] / 340.0f + 36.53f;
114 
115  float gyro_x = data[4] * MPU6050_SCALE_DPS_PER_DIGIT_2000;
116  float gyro_y = data[5] * MPU6050_SCALE_DPS_PER_DIGIT_2000;
117  float gyro_z = data[6] * MPU6050_SCALE_DPS_PER_DIGIT_2000;
118 
119  ESP_LOGD(TAG,
120  "Got accel={x=%.3f m/s², y=%.3f m/s², z=%.3f m/s²}, "
121  "gyro={x=%.3f °/s, y=%.3f °/s, z=%.3f °/s}, temp=%.3f°C",
122  accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z, temperature);
123 
124  if (this->accel_x_sensor_ != nullptr)
125  this->accel_x_sensor_->publish_state(accel_x);
126  if (this->accel_y_sensor_ != nullptr)
127  this->accel_y_sensor_->publish_state(accel_y);
128  if (this->accel_z_sensor_ != nullptr)
129  this->accel_z_sensor_->publish_state(accel_z);
130 
131  if (this->temperature_sensor_ != nullptr)
132  this->temperature_sensor_->publish_state(temperature);
133 
134  if (this->gyro_x_sensor_ != nullptr)
135  this->gyro_x_sensor_->publish_state(gyro_x);
136  if (this->gyro_y_sensor_ != nullptr)
137  this->gyro_y_sensor_->publish_state(gyro_y);
138  if (this->gyro_z_sensor_ != nullptr)
139  this->gyro_z_sensor_->publish_state(gyro_z);
140 
141  this->status_clear_warning();
142 }
144 
145 } // namespace mpu6050
146 } // namespace esphome
const uint8_t MPU6050_RANGE_2G
Definition: mpu6050.cpp:17
bool read_byte(uint8_t a_register, uint8_t *data, bool stop=true)
Definition: i2c.h:96
const float DATA
For components that import data from directly connected sensors like DHT.
Definition: component.cpp:18
sensor::Sensor * gyro_y_sensor_
Definition: mpu6050.h:33
const float GRAVITY_EARTH
Definition: mpu6050.cpp:21
sensor::Sensor * accel_z_sensor_
Definition: mpu6050.h:30
const uint8_t MPU6050_BIT_SLEEP_ENABLED
Definition: mpu6050.cpp:19
const uint8_t MPU6050_REGISTER_ACCEL_XOUT_H
Definition: mpu6050.cpp:13
float temperature
Definition: qmp6988.h:71
sensor::Sensor * accel_y_sensor_
Definition: mpu6050.h:29
const uint8_t MPU6050_REGISTER_ACCEL_CONFIG
Definition: mpu6050.cpp:12
sensor::Sensor * gyro_z_sensor_
Definition: mpu6050.h:34
const float MPU6050_RANGE_PER_DIGIT_2G
Definition: mpu6050.cpp:18
void status_clear_warning()
Definition: component.cpp:149
const uint8_t MPU6050_BIT_TEMPERATURE_DISABLED
Definition: mpu6050.cpp:20
void publish_state(float state)
Publish a new state to the front-end.
Definition: sensor.cpp:72
sensor::Sensor * temperature_sensor_
Definition: mpu6050.h:31
sensor::Sensor * accel_x_sensor_
Definition: mpu6050.h:28
void status_set_warning()
Definition: component.cpp:141
const float MPU6050_SCALE_DPS_PER_DIGIT_2000
Definition: mpu6050.cpp:16
float get_setup_priority() const override
Definition: mpu6050.cpp:143
bool write_byte(uint8_t a_register, uint8_t data, bool stop=true)
Definition: i2c.h:123
virtual void mark_failed()
Mark this component as failed.
Definition: component.cpp:112
const uint8_t MPU6050_REGISTER_GYRO_CONFIG
Definition: mpu6050.cpp:11
sensor::Sensor * gyro_x_sensor_
Definition: mpu6050.h:32
Definition: a4988.cpp:4
const uint8_t MPU6050_CLOCK_SOURCE_X_GYRO
Definition: mpu6050.cpp:14
const uint8_t MPU6050_SCALE_2000_DPS
Definition: mpu6050.cpp:15
const uint8_t MPU6050_REGISTER_POWER_MANAGEMENT_1
Definition: mpu6050.cpp:10
const uint8_t MPU6050_REGISTER_WHO_AM_I
Definition: mpu6050.cpp:9
bool read_bytes_16(uint8_t a_register, uint16_t *data, uint8_t len)
Definition: i2c.h:88