175 lines
6.1 KiB
C++
175 lines
6.1 KiB
C++
#include "qmc5883l.h"
|
|
#include "esphome/core/application.h"
|
|
#include "esphome/core/log.h"
|
|
#include "esphome/core/hal.h"
|
|
#include <cmath>
|
|
|
|
namespace esphome {
|
|
namespace qmc5883l {
|
|
|
|
static const char *const TAG = "qmc5883l";
|
|
static const uint8_t QMC5883L_ADDRESS = 0x0D;
|
|
|
|
static const uint8_t QMC5883L_REGISTER_DATA_X_LSB = 0x00;
|
|
static const uint8_t QMC5883L_REGISTER_DATA_X_MSB = 0x01;
|
|
static const uint8_t QMC5883L_REGISTER_DATA_Y_LSB = 0x02;
|
|
static const uint8_t QMC5883L_REGISTER_DATA_Y_MSB = 0x03;
|
|
static const uint8_t QMC5883L_REGISTER_DATA_Z_LSB = 0x04;
|
|
static const uint8_t QMC5883L_REGISTER_DATA_Z_MSB = 0x05;
|
|
static const uint8_t QMC5883L_REGISTER_STATUS = 0x06;
|
|
static const uint8_t QMC5883L_REGISTER_TEMPERATURE_LSB = 0x07;
|
|
static const uint8_t QMC5883L_REGISTER_TEMPERATURE_MSB = 0x08;
|
|
static const uint8_t QMC5883L_REGISTER_CONTROL_1 = 0x09;
|
|
static const uint8_t QMC5883L_REGISTER_CONTROL_2 = 0x0A;
|
|
static const uint8_t QMC5883L_REGISTER_PERIOD = 0x0B;
|
|
|
|
void QMC5883LComponent::setup() {
|
|
ESP_LOGCONFIG(TAG, "Running setup");
|
|
// Soft Reset
|
|
if (!this->write_byte(QMC5883L_REGISTER_CONTROL_2, 1 << 7)) {
|
|
this->error_code_ = COMMUNICATION_FAILED;
|
|
this->mark_failed();
|
|
return;
|
|
}
|
|
delay(10);
|
|
|
|
uint8_t control_1 = 0;
|
|
control_1 |= 0b01 << 0; // MODE (Mode) -> 0b00=standby, 0b01=continuous
|
|
control_1 |= this->datarate_ << 2;
|
|
control_1 |= this->range_ << 4;
|
|
control_1 |= this->oversampling_ << 6;
|
|
if (!this->write_byte(QMC5883L_REGISTER_CONTROL_1, control_1)) {
|
|
this->error_code_ = COMMUNICATION_FAILED;
|
|
this->mark_failed();
|
|
return;
|
|
}
|
|
|
|
uint8_t control_2 = 0;
|
|
control_2 |= 0b0 << 7; // SOFT_RST (Soft Reset) -> 0b00=disabled, 0b01=enabled
|
|
control_2 |= 0b0 << 6; // ROL_PNT (Pointer Roll Over) -> 0b00=disabled, 0b01=enabled
|
|
control_2 |= 0b0 << 0; // INT_ENB (Interrupt) -> 0b00=disabled, 0b01=enabled
|
|
if (!this->write_byte(QMC5883L_REGISTER_CONTROL_2, control_2)) {
|
|
this->error_code_ = COMMUNICATION_FAILED;
|
|
this->mark_failed();
|
|
return;
|
|
}
|
|
|
|
uint8_t period = 0x01; // recommended value
|
|
if (!this->write_byte(QMC5883L_REGISTER_PERIOD, period)) {
|
|
this->error_code_ = COMMUNICATION_FAILED;
|
|
this->mark_failed();
|
|
return;
|
|
}
|
|
|
|
if (this->get_update_interval() < App.get_loop_interval()) {
|
|
high_freq_.start();
|
|
}
|
|
}
|
|
void QMC5883LComponent::dump_config() {
|
|
ESP_LOGCONFIG(TAG, "QMC5883L:");
|
|
LOG_I2C_DEVICE(this);
|
|
if (this->error_code_ == COMMUNICATION_FAILED) {
|
|
ESP_LOGE(TAG, ESP_LOG_MSG_COMM_FAIL);
|
|
}
|
|
LOG_UPDATE_INTERVAL(this);
|
|
|
|
LOG_SENSOR(" ", "X Axis", this->x_sensor_);
|
|
LOG_SENSOR(" ", "Y Axis", this->y_sensor_);
|
|
LOG_SENSOR(" ", "Z Axis", this->z_sensor_);
|
|
LOG_SENSOR(" ", "Heading", this->heading_sensor_);
|
|
LOG_SENSOR(" ", "Temperature", this->temperature_sensor_);
|
|
}
|
|
float QMC5883LComponent::get_setup_priority() const { return setup_priority::DATA; }
|
|
void QMC5883LComponent::update() {
|
|
i2c::ErrorCode err;
|
|
uint8_t status = false;
|
|
// Status byte gets cleared when data is read, so we have to read this first.
|
|
// If status and two axes are desired, it's possible to save one byte of traffic by enabling
|
|
// ROL_PNT in setup and reading 7 bytes starting at the status register.
|
|
// If status and all three axes are desired, using ROL_PNT saves you 3 bytes.
|
|
// But simply not reading status saves you 4 bytes always and is much simpler.
|
|
if (ESPHOME_LOG_LEVEL >= ESPHOME_LOG_LEVEL_DEBUG) {
|
|
err = this->read_register(QMC5883L_REGISTER_STATUS, &status, 1);
|
|
if (err != i2c::ERROR_OK) {
|
|
this->status_set_warning(str_sprintf("status read failed (%d)", err).c_str());
|
|
return;
|
|
}
|
|
}
|
|
|
|
uint16_t raw[3] = {0};
|
|
// Z must always be requested, otherwise the data registers will remain locked against updates.
|
|
// Skipping the Y axis if X and Z are needed actually requires an additional byte of comms.
|
|
// Starting partway through the axes does save you traffic.
|
|
uint8_t start, dest;
|
|
if (this->heading_sensor_ != nullptr || this->x_sensor_ != nullptr) {
|
|
start = QMC5883L_REGISTER_DATA_X_LSB;
|
|
dest = 0;
|
|
} else if (this->y_sensor_ != nullptr) {
|
|
start = QMC5883L_REGISTER_DATA_Y_LSB;
|
|
dest = 1;
|
|
} else {
|
|
start = QMC5883L_REGISTER_DATA_Z_LSB;
|
|
dest = 2;
|
|
}
|
|
err = this->read_bytes_16_le_(start, &raw[dest], 3 - dest);
|
|
if (err != i2c::ERROR_OK) {
|
|
this->status_set_warning(str_sprintf("mag read failed (%d)", err).c_str());
|
|
return;
|
|
}
|
|
|
|
float mg_per_bit;
|
|
switch (this->range_) {
|
|
case QMC5883L_RANGE_200_UT:
|
|
mg_per_bit = 0.0833f;
|
|
break;
|
|
case QMC5883L_RANGE_800_UT:
|
|
mg_per_bit = 0.333f;
|
|
break;
|
|
default:
|
|
mg_per_bit = NAN;
|
|
}
|
|
|
|
// in µT
|
|
const float x = int16_t(raw[0]) * mg_per_bit * 0.1f;
|
|
const float y = int16_t(raw[1]) * mg_per_bit * 0.1f;
|
|
const float z = int16_t(raw[2]) * mg_per_bit * 0.1f;
|
|
|
|
float heading = atan2f(0.0f - x, y) * 180.0f / M_PI;
|
|
|
|
float temp = NAN;
|
|
if (this->temperature_sensor_ != nullptr) {
|
|
uint16_t raw_temp;
|
|
err = this->read_bytes_16_le_(QMC5883L_REGISTER_TEMPERATURE_LSB, &raw_temp);
|
|
if (err != i2c::ERROR_OK) {
|
|
this->status_set_warning(str_sprintf("temp read failed (%d)", err).c_str());
|
|
return;
|
|
}
|
|
temp = int16_t(raw_temp) * 0.01f;
|
|
}
|
|
|
|
ESP_LOGD(TAG, "Got x=%0.02fµT y=%0.02fµT z=%0.02fµT heading=%0.01f° temperature=%0.01f°C status=%u", x, y, z, heading,
|
|
temp, status);
|
|
|
|
if (this->x_sensor_ != nullptr)
|
|
this->x_sensor_->publish_state(x);
|
|
if (this->y_sensor_ != nullptr)
|
|
this->y_sensor_->publish_state(y);
|
|
if (this->z_sensor_ != nullptr)
|
|
this->z_sensor_->publish_state(z);
|
|
if (this->heading_sensor_ != nullptr)
|
|
this->heading_sensor_->publish_state(heading);
|
|
if (this->temperature_sensor_ != nullptr)
|
|
this->temperature_sensor_->publish_state(temp);
|
|
}
|
|
|
|
i2c::ErrorCode QMC5883LComponent::read_bytes_16_le_(uint8_t a_register, uint16_t *data, uint8_t len) {
|
|
i2c::ErrorCode err = this->read_register(a_register, reinterpret_cast<uint8_t *>(data), len * 2);
|
|
if (err != i2c::ERROR_OK)
|
|
return err;
|
|
for (size_t i = 0; i < len; i++)
|
|
data[i] = convert_little_endian(data[i]);
|
|
return err;
|
|
}
|
|
|
|
} // namespace qmc5883l
|
|
} // namespace esphome
|