diff --git a/components/bmm150/README.md b/components/bmm150/README.md index 5a407a7c..1ecb7e84 100644 --- a/components/bmm150/README.md +++ b/components/bmm150/README.md @@ -1,22 +1,101 @@ +# BMM150 + +Bosch BMM150 3-axis magnetometer. Publishes raw XYZ and a vehicle heading. + +M5Stack Unit GNSS uses address **0x10**. For a compass, set `update_interval` to **200ms–1s**. + +## Options + +| Key | Default | Description | +|---|---|---| +| `address` | `0x10` | I2C address (0x10–0x13, CSB/SDO strap) | +| `update_interval` | `60s` | Poll period. Use 200ms–1s for heading | +| `magnetic_field_x/y/z` | — | Magnetic field in µT (Bosch integer compensation; **unverified on hardware**) | +| `heading` | — | True heading 0–360°. Stays `unknown` until a valid calibration is stored | +| `accel_x_id` / `accel_y_id` / `accel_z_id` | — | Accel sensors for tilt compensation. If omitted, planar `atan2(-my, mx)` is used. If set but the accel has no valid sample, heading stays `unknown` (no silent planar fallback) | +| `declination` | `0` | Magnetic declination in degrees. Korea is about **-8** (west). Not hardcoded | +| `heading_offset` | `0` | Mounting offset in degrees (module +X vs vehicle forward). Added after declination | +| `soft_iron` | `false` | Per-axis scale correction. Leave off for in-place yaw calibration | +| `calibration_mode` | `yaw` | `yaw`: in-place full circle, XY only. `full`: figure-8 including Z | +| `mag_axes` | `[x, y, z]` | Mag axis remap (`x`,`y`,`z`,`-x`,`-y`,`-z`). **PCB alignment unverified** | +| `accel_axes` | `[x, y, z]` | Accel axis remap. **PCB alignment unverified** | +| `on_calibration_finished` | — | Trigger after calibration. `success` (bool) | + +## Calibration + +A dashboard has hard-iron offset from steel and speakers. Heading is not published without a stored calibration. Calibrate **in the installed position** so that offset includes the vehicle. + +### In the vehicle (`calibration_mode: yaw`, default) + +1. Leave the module mounted. +2. Run `bmm150.calibrate` and **slowly drive or turn the vehicle through a full circle in place**. +3. X and Y min/max delta must each exceed **20 µT**. If Z delta also exceeds **5 µT** (typical on a tilted dash), the Z offset is updated; otherwise the previous Z offset is kept. +4. A level mount cannot excite Z by yaw rotation — that is expected. + +### Off the vehicle (`calibration_mode: full`) + +1. Remove the module and rotate it in a slow **figure-8** so all three axes move. +2. XY delta must exceed **20 µT**, Z delta **5 µT**. +3. This does **not** capture dash-plate hard-iron. Use it only to map axes, then redo `yaw` once reinstalled. + +`duration` must cover several `update_interval` samples. With the schema default of 60s and `duration: 30s`, calibration will always be rejected. + +Changing `mag_axes` or the internal µT scale invalidates the stored blob; recalibrate. + +```yaml +on_...: + - bmm150.calibrate: + id: mag + duration: 30s +``` + +## Vehicle install + +Keep the module away from speaker magnets, steel panels, and high-current wiring. Mounting the GNSS unit on a steel dash can add tens of µT of offset. + +If the module is not aligned with vehicle forward, set `heading_offset` (degrees, clockwise from module +X to the nose of the car). Do not reuse `mag_axes` for that — those options are for chip-to-chip axis swap. + +## Axis alignment and declination + +Tilt compensation assumes mag and accel X/Y/Z point the same way. On M5 Unit GNSS the BMM150 and BMI270 may be rotated relative to each other — measure, then set `mag_axes` / `accel_axes`. Recalibrate after changing those lists. + +For true north, set `declination`. Seoul is about `-8`. + +## Example + ```yaml external_components: - source: github://eigger/espcomponents@latest components: [ bmm150 ] - + i2c: sda: GPIO21 scl: GPIO22 scan: true - + sensor: - platform: bmm150 + id: mag address: 0x10 - update_interval: 30s + update_interval: 500ms magnetic_field_x: name: "MAG X" magnetic_field_y: name: "MAG Y" magnetic_field_z: name: "MAG Z" - + heading: + name: "Heading" + accel_x_id: id_gnss_bmi270_accel_x + accel_y_id: id_gnss_bmi270_accel_y + accel_z_id: id_gnss_bmi270_accel_z + declination: -8 + heading_offset: 0 + calibration_mode: yaw + mag_axes: [x, y, z] + accel_axes: [x, y, z] + on_calibration_finished: + - logger.log: + format: "BMM150 calibration %s" + args: ['success ? "ok" : "failed"'] ``` diff --git a/components/bmm150/bmm150.cpp b/components/bmm150/bmm150.cpp index 38e20681..eefb74dd 100644 --- a/components/bmm150/bmm150.cpp +++ b/components/bmm150/bmm150.cpp @@ -1,23 +1,58 @@ #include "bmm150.h" #include "esphome/core/log.h" #include "esphome/core/hal.h" +#include "esphome/core/helpers.h" +#include +#include #include +#include namespace esphome { namespace bmm150 { static const char *TAG = "bmm150"; +// B-1 unverified: Bosch integer compensate_*() is documented as µT. Confirm on hardware +// (horizontal circle radius ~30 µT in Seoul, |B| ~50 µT). Change this if the scale is wrong. +static constexpr float MAG_UT_SCALE = 1.0f; +static constexpr float CAL_MIN_DELTA_XY_UT = 20.0f; +static constexpr float CAL_MIN_DELTA_Z_UT = 5.0f; +static constexpr uint8_t CALIB_VERSION = 1; +static constexpr uint8_t RESET_FAILURE_THRESHOLD = 5; + int8_t reg_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, void *intf_ptr); int8_t reg_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t length, void *intf_ptr); void delay_us(uint32_t period_us, void *intf_ptr); static bool is_overflow(int16_t value) { return value == BMM150_OVERFLOW_OUTPUT; } +void BMM150Component::set_mag_axes(uint8_t x_src, int8_t x_sign, uint8_t y_src, int8_t y_sign, uint8_t z_src, + int8_t z_sign) { + this->mag_axes_.src[0] = x_src; + this->mag_axes_.sign[0] = x_sign; + this->mag_axes_.src[1] = y_src; + this->mag_axes_.sign[1] = y_sign; + this->mag_axes_.src[2] = z_src; + this->mag_axes_.sign[2] = z_sign; +} + +void BMM150Component::set_accel_axes(uint8_t x_src, int8_t x_sign, uint8_t y_src, int8_t y_sign, uint8_t z_src, + int8_t z_sign) { + this->accel_axes_.src[0] = x_src; + this->accel_axes_.sign[0] = x_sign; + this->accel_axes_.src[1] = y_src; + this->accel_axes_.sign[1] = y_sign; + this->accel_axes_.src[2] = z_src; + this->accel_axes_.sign[2] = z_sign; +} + void BMM150Component::setup() { + this->load_calibration_(); int8_t code = this->bmm150_initialization(); if (code == BMM150_OK) { this->initialized_ = true; + this->consecutive_failures_ = 0; + this->init_retry_logged_ = false; return; } // Wrong/missing chip ID is definitive. Bus NAKs during boot are not — this bus @@ -28,6 +63,7 @@ void BMM150Component::setup() { return; } ESP_LOGW(TAG, "Init failed (%d), will retry", code); + this->init_retry_logged_ = true; this->status_set_warning(); } @@ -45,6 +81,30 @@ void BMM150Component::dump_config() { LOG_SENSOR(" ", "Magnetic Field X", this->mag_x_); LOG_SENSOR(" ", "Magnetic Field Y", this->mag_y_); LOG_SENSOR(" ", "Magnetic Field Z", this->mag_z_); + LOG_SENSOR(" ", "Heading", this->heading_); + ESP_LOGCONFIG(TAG, " Declination: %.1f°", this->declination_); + ESP_LOGCONFIG(TAG, " Heading offset: %.1f°", this->heading_offset_); + ESP_LOGCONFIG(TAG, " Soft-iron: %s", YESNO(this->soft_iron_)); + ESP_LOGCONFIG(TAG, " Calibration mode: %s", + this->calibration_mode_ == CALIBRATION_MODE_FULL ? "full (figure-8)" : "yaw (in-place turn)"); + ESP_LOGCONFIG(TAG, " Tilt compensation: %s", + this->has_accel_ids_() ? "required (heading unknown until accel publishes)" + : "disabled (planar fallback)"); + // B-4 unverified: identity maps assume mag and accel axes are parallel on the PCB. + ESP_LOGCONFIG(TAG, " Mag axes: %c%c %c%c %c%c (unverified)", this->mag_axes_.sign[0] < 0 ? '-' : '+', + "XYZ"[this->mag_axes_.src[0]], this->mag_axes_.sign[1] < 0 ? '-' : '+', "XYZ"[this->mag_axes_.src[1]], + this->mag_axes_.sign[2] < 0 ? '-' : '+', "XYZ"[this->mag_axes_.src[2]]); + ESP_LOGCONFIG(TAG, " Accel axes: %c%c %c%c %c%c (unverified)", this->accel_axes_.sign[0] < 0 ? '-' : '+', + "XYZ"[this->accel_axes_.src[0]], this->accel_axes_.sign[1] < 0 ? '-' : '+', + "XYZ"[this->accel_axes_.src[1]], this->accel_axes_.sign[2] < 0 ? '-' : '+', + "XYZ"[this->accel_axes_.src[2]]); + if (this->calib_.valid == 1) { + ESP_LOGCONFIG(TAG, " Calibration: offset=(%d,%d,%d) scale=(%.3f,%.3f,%.3f)", this->calib_.offset_x, + this->calib_.offset_y, this->calib_.offset_z, this->calib_.scale_x, this->calib_.scale_y, + this->calib_.scale_z); + } else { + ESP_LOGW(TAG, " Calibration: not stored (heading will stay unknown until bmm150.calibrate)"); + } } float BMM150Component::get_setup_priority() const { return setup_priority::DATA; } @@ -61,11 +121,16 @@ void BMM150Component::update() { return; } if (code != BMM150_OK) { - ESP_LOGW(TAG, "Init retry failed (%d)", code); + if (!this->init_retry_logged_) { + ESP_LOGW(TAG, "Init retry failed (%d); will keep retrying quietly", code); + this->init_retry_logged_ = true; + } this->status_set_warning(); return; } this->initialized_ = true; + this->consecutive_failures_ = 0; + this->init_retry_logged_ = false; ESP_LOGI(TAG, "Initialized after retry"); } @@ -75,24 +140,248 @@ void BMM150Component::update() { // transaction. The callback latch covers any I2C failure in this call. if (code != BMM150_OK || this->bus_error_) { ESP_LOGW(TAG, "Read failed (rslt=%d)", code); - this->status_set_warning(); + this->note_failure_(); return; } if (is_overflow(mag_data_.x) || is_overflow(mag_data_.y) || is_overflow(mag_data_.z)) { ESP_LOGW(TAG, "Compensation overflow (x=%d y=%d z=%d)", mag_data_.x, mag_data_.y, mag_data_.z); - this->status_set_warning(); + this->note_failure_(); return; } + this->consecutive_failures_ = 0; this->status_clear_warning(); + float raw[3] = {mag_data_.x * MAG_UT_SCALE, mag_data_.y * MAG_UT_SCALE, mag_data_.z * MAG_UT_SCALE}; + float mag[3]; + this->apply_axes_(raw, this->mag_axes_, mag); + if (this->mag_x_ != nullptr) - this->mag_x_->publish_state(mag_data_.x); + this->mag_x_->publish_state(mag[0]); if (this->mag_y_ != nullptr) - this->mag_y_->publish_state(mag_data_.y); + this->mag_y_->publish_state(mag[1]); if (this->mag_z_ != nullptr) - this->mag_z_->publish_state(mag_data_.z); + this->mag_z_->publish_state(mag[2]); + + if (this->calibrating_) { + for (int i = 0; i < 3; i++) { + if (mag[i] < this->cal_min_[i]) + this->cal_min_[i] = mag[i]; + if (mag[i] > this->cal_max_[i]) + this->cal_max_[i] = mag[i]; + } + if (this->heading_ != nullptr) + this->heading_->publish_state(NAN); + return; + } + + if (this->heading_ == nullptr) + return; + + if (this->calib_.valid != 1) { + this->heading_->publish_state(NAN); + return; + } + + float mx = (mag[0] - this->calib_.offset_x) * this->calib_.scale_x; + float my = (mag[1] - this->calib_.offset_y) * this->calib_.scale_y; + float mz = (mag[2] - this->calib_.offset_z) * this->calib_.scale_z; + + float heading; + if (this->has_accel_ids_()) { + float accel[3]; + if (!this->read_accel_(accel)) { + if (!this->tilt_unavailable_logged_) { + ESP_LOGW(TAG, "Accel IDs set but no valid accel sample; heading unknown (not using planar fallback)"); + this->tilt_unavailable_logged_ = true; + } + this->heading_->publish_state(NAN); + return; + } + if (this->tilt_unavailable_logged_) { + ESP_LOGI(TAG, "Accel samples restored; tilt compensation active"); + this->tilt_unavailable_logged_ = false; + } + heading = this->compute_tilt_heading_(mx, my, mz, accel); + } else { + heading = this->compute_planar_heading_(mx, my); + } + this->heading_->publish_state(heading); +} + +void BMM150Component::start_calibration(uint32_t duration_ms) { + this->calibrating_ = true; + for (int i = 0; i < 3; i++) { + this->cal_min_[i] = 10000.0f; + this->cal_max_[i] = -10000.0f; + } + this->cancel_timeout("bmm150_cal"); + this->set_timeout("bmm150_cal", duration_ms, [this]() { this->finish_calibration_(); }); + this->tilt_unavailable_logged_ = false; + ESP_LOGI(TAG, "Calibration started (%" PRIu32 " ms); %s", duration_ms, + this->calibration_mode_ == CALIBRATION_MODE_FULL + ? "rotate the device in a figure-8" + : "turn the vehicle slowly in place through a full circle"); +} + +void BMM150Component::note_failure_() { + this->status_set_warning(); + if (++this->consecutive_failures_ < RESET_FAILURE_THRESHOLD) + return; + ESP_LOGW(TAG, "Sensor appears to have reset; re-initializing"); + this->initialized_ = false; + this->consecutive_failures_ = 0; +} + +void BMM150Component::finish_calibration_() { + this->calibrating_ = false; + float dx = this->cal_max_[0] - this->cal_min_[0]; + float dy = this->cal_max_[1] - this->cal_min_[1]; + float dz = this->cal_max_[2] - this->cal_min_[2]; + const bool yaw_only = this->calibration_mode_ == CALIBRATION_MODE_YAW; + if (dx < CAL_MIN_DELTA_XY_UT || dy < CAL_MIN_DELTA_XY_UT || (!yaw_only && dz < CAL_MIN_DELTA_Z_UT)) { + if (yaw_only) { + ESP_LOGW(TAG, "Calibration rejected: axis delta (%.1f, %.1f, %.1f) µT, need XY>%.0f", dx, dy, dz, + CAL_MIN_DELTA_XY_UT); + } else { + ESP_LOGW(TAG, "Calibration rejected: axis delta (%.1f, %.1f, %.1f) µT, need XY>%.0f, Z>%.0f", dx, dy, dz, + CAL_MIN_DELTA_XY_UT, CAL_MIN_DELTA_Z_UT); + } + this->calibration_finished_trigger_.trigger(false); + return; + } + + this->stamp_calibration_context_(&this->calib_); + this->calib_.offset_x = (int16_t) ((this->cal_max_[0] + this->cal_min_[0]) / 2.0f); + this->calib_.offset_y = (int16_t) ((this->cal_max_[1] + this->cal_min_[1]) / 2.0f); + if (yaw_only && dz < CAL_MIN_DELTA_Z_UT) { + ESP_LOGI(TAG, "Yaw calibration: Z delta %.1f µT too small, keeping previous Z offset", dz); + } else { + this->calib_.offset_z = (int16_t) ((this->cal_max_[2] + this->cal_min_[2]) / 2.0f); + } + if (this->soft_iron_) { + if (yaw_only) { + float avg = (dx + dy) / 2.0f; + this->calib_.scale_x = avg / dx; + this->calib_.scale_y = avg / dy; + this->calib_.scale_z = 1.0f; + } else { + float avg = (dx + dy + dz) / 3.0f; + this->calib_.scale_x = avg / dx; + this->calib_.scale_y = avg / dy; + this->calib_.scale_z = avg / dz; + } + } else { + this->calib_.scale_x = this->calib_.scale_y = this->calib_.scale_z = 1.0f; + } + this->calib_.valid = 1; + this->save_calibration_(); + ESP_LOGI(TAG, "Calibration saved: offset=(%d,%d,%d) scale=(%.3f,%.3f,%.3f)", this->calib_.offset_x, + this->calib_.offset_y, this->calib_.offset_z, this->calib_.scale_x, this->calib_.scale_y, + this->calib_.scale_z); + this->calibration_finished_trigger_.trigger(true); +} + +void BMM150Component::reset_calibration_() { + memset(&this->calib_, 0, sizeof(this->calib_)); + this->calib_.scale_x = this->calib_.scale_y = this->calib_.scale_z = 1.0f; + this->stamp_calibration_context_(&this->calib_); +} + +void BMM150Component::stamp_calibration_context_(BMM150Calibration *out) const { + out->version = CALIB_VERSION; + out->mag_ut_scale = MAG_UT_SCALE; + for (int i = 0; i < 3; i++) { + out->mag_src[i] = this->mag_axes_.src[i]; + out->mag_sign[i] = this->mag_axes_.sign[i]; + } +} + +bool BMM150Component::calibration_matches_config_(const BMM150Calibration &c) const { + if (c.valid != 1 || c.version != CALIB_VERSION) + return false; + if (c.mag_ut_scale != MAG_UT_SCALE) + return false; + for (int i = 0; i < 3; i++) { + if (c.mag_src[i] != this->mag_axes_.src[i] || c.mag_sign[i] != this->mag_axes_.sign[i]) + return false; + } + return true; +} + +void BMM150Component::load_calibration_() { + // Component is not an EntityBase, so get_object_id_hash() is unavailable. + // Version is in the key so older blobs are not decoded as this struct. + uint32_t hash = fnv1_hash(str_sprintf("bmm150_cal_v%u_%02X", CALIB_VERSION, this->address_)); + this->pref_ = global_preferences->make_preference(hash, true); + this->reset_calibration_(); + BMM150Calibration loaded{}; + if (this->pref_.load(&loaded) && this->calibration_matches_config_(loaded)) { + this->calib_ = loaded; + ESP_LOGI(TAG, "Loaded calibration offset=(%d,%d,%d) scale=(%.3f,%.3f,%.3f)", this->calib_.offset_x, + this->calib_.offset_y, this->calib_.offset_z, this->calib_.scale_x, this->calib_.scale_y, + this->calib_.scale_z); + } else if (loaded.valid == 1) { + ESP_LOGW(TAG, "Stored calibration ignored (version/axes/scale mismatch); heading unknown until recalibrated"); + } else { + ESP_LOGW(TAG, "No stored calibration; heading will stay unknown until bmm150.calibrate succeeds"); + } +} + +void BMM150Component::save_calibration_() { + if (!this->pref_.save(&this->calib_)) { + ESP_LOGW(TAG, "Failed to save calibration"); + } +} + +void BMM150Component::apply_axes_(const float in[3], const BMM150AxisMap &map, float out[3]) const { + out[0] = map.sign[0] * in[map.src[0]]; + out[1] = map.sign[1] * in[map.src[1]]; + out[2] = map.sign[2] * in[map.src[2]]; +} + +bool BMM150Component::has_accel_ids_() const { + return this->accel_x_ != nullptr && this->accel_y_ != nullptr && this->accel_z_ != nullptr; +} + +bool BMM150Component::read_accel_(float accel[3]) const { + if (!this->has_accel_ids_()) + return false; + if (!this->accel_x_->has_state() || !this->accel_y_->has_state() || !this->accel_z_->has_state()) + return false; + float in[3] = {this->accel_x_->state, this->accel_y_->state, this->accel_z_->state}; + if (std::isnan(in[0]) || std::isnan(in[1]) || std::isnan(in[2])) + return false; + this->apply_axes_(in, this->accel_axes_, accel); + return true; +} + +float BMM150Component::wrap_degrees_(float deg) { + deg = fmodf(deg, 360.0f); + if (deg < 0.0f) + deg += 360.0f; + return deg; +} + +float BMM150Component::apply_heading_offsets_(float magnetic_heading) const { + return wrap_degrees_(magnetic_heading + this->declination_ + this->heading_offset_); +} + +float BMM150Component::compute_planar_heading_(float mx, float my) const { + return this->apply_heading_offsets_(atan2f(-my, mx) * (180.0f / std::numbers::pi_v)); +} + +float BMM150Component::compute_tilt_heading_(float mx, float my, float mz, const float accel[3]) const { + // Standard tilt-compensated compass. Valid only if mag/accel axes are aligned (see mag_axes/accel_axes). + const float ax = accel[0]; + const float ay = accel[1]; + const float az = accel[2]; + const float roll = atan2f(ay, az); + const float pitch = atan2f(-ax, ay * sinf(roll) + az * cosf(roll)); + const float xh = mx * cosf(pitch) + mz * sinf(pitch); + const float yh = mx * sinf(roll) * sinf(pitch) + my * cosf(roll) - mz * sinf(roll) * cosf(pitch); + return this->apply_heading_offsets_(atan2f(-yh, xh) * (180.0f / std::numbers::pi_v)); } int8_t BMM150Component::bmm150_initialization() { @@ -109,12 +398,13 @@ int8_t BMM150Component::bmm150_initialization() { // bmm150_init() only sets dev_.chip_id on ID match but still returns BMM150_OK otherwise. if (rslt != BMM150_OK) return rslt; - if (dev_.chip_id != BMM150_CHIP_ID) - return BMM150_E_DEV_NOT_FOUND; - // read_trim_registers() runs three reads; intf_rslt only reflects the last one and partial - // failures still commit zeroed trim_data. Latch any callback failure instead. + // A NAK leaves chip_id at 0; that is not proof the chip is missing. Classify bus + // failure first so setup()/update() can retry instead of mark_failed(). if (this->bus_error_) return BMM150_E_COM_FAIL; + // Bus ACKed but ID is not 0x32: wrong chip or address collision. + if (dev_.chip_id != BMM150_CHIP_ID) + return BMM150_E_DEV_NOT_FOUND; struct bmm150_settings settings; settings.pwr_mode = BMM150_POWERMODE_NORMAL; diff --git a/components/bmm150/bmm150.h b/components/bmm150/bmm150.h index 7ee074a4..7e73bae4 100644 --- a/components/bmm150/bmm150.h +++ b/components/bmm150/bmm150.h @@ -1,7 +1,9 @@ -#ifndef __BMM150_H__ -#define __BMM150_H__ +#pragma once +#include "esphome/core/automation.h" #include "esphome/core/component.h" +#include "esphome/core/helpers.h" +#include "esphome/core/preferences.h" #include "esphome/components/sensor/sensor.h" #include "esphome/components/i2c/i2c.h" #include "bmm150_lib.h" @@ -9,11 +11,47 @@ namespace esphome { namespace bmm150 { +enum CalibrationMode : uint8_t { + CALIBRATION_MODE_YAW = 0, + CALIBRATION_MODE_FULL = 1, +}; + +struct BMM150Calibration { + uint8_t version; + uint8_t mag_src[3]; + int8_t mag_sign[3]; + float mag_ut_scale; + int16_t offset_x; + int16_t offset_y; + int16_t offset_z; + float scale_x; + float scale_y; + float scale_z; + uint8_t valid; +} PACKED; + +struct BMM150AxisMap { + uint8_t src[3]{0, 1, 2}; + int8_t sign[3]{1, 1, 1}; +}; + class BMM150Component : public PollingComponent, public i2c::I2CDevice { public: void set_mag_x(sensor::Sensor *mag_x) { mag_x_ = mag_x; } void set_mag_y(sensor::Sensor *mag_y) { mag_y_ = mag_y; } void set_mag_z(sensor::Sensor *mag_z) { mag_z_ = mag_z; } + void set_heading(sensor::Sensor *heading) { heading_ = heading; } + + void set_accel_x(sensor::Sensor *accel_x) { accel_x_ = accel_x; } + void set_accel_y(sensor::Sensor *accel_y) { accel_y_ = accel_y; } + void set_accel_z(sensor::Sensor *accel_z) { accel_z_ = accel_z; } + + void set_declination(float declination) { declination_ = declination; } + void set_heading_offset(float heading_offset) { heading_offset_ = heading_offset; } + void set_soft_iron(bool soft_iron) { soft_iron_ = soft_iron; } + void set_calibration_mode(CalibrationMode mode) { calibration_mode_ = mode; } + void set_mag_axes(uint8_t x_src, int8_t x_sign, uint8_t y_src, int8_t y_sign, uint8_t z_src, int8_t z_sign); + void set_accel_axes(uint8_t x_src, int8_t x_sign, uint8_t y_src, int8_t y_sign, uint8_t z_src, int8_t z_sign); void setup() override; void dump_config() override; @@ -21,20 +59,67 @@ class BMM150Component : public PollingComponent, public i2c::I2CDevice { void update() override; void set_bus_error() { this->bus_error_ = true; } + void start_calibration(uint32_t duration_ms); + Trigger *get_calibration_finished_trigger() { return &this->calibration_finished_trigger_; } protected: sensor::Sensor *mag_x_{nullptr}; sensor::Sensor *mag_y_{nullptr}; sensor::Sensor *mag_z_{nullptr}; + sensor::Sensor *heading_{nullptr}; + sensor::Sensor *accel_x_{nullptr}; + sensor::Sensor *accel_y_{nullptr}; + sensor::Sensor *accel_z_{nullptr}; struct bmm150_dev dev_; struct bmm150_mag_data mag_data_; bool bus_error_{false}; bool initialized_{false}; + uint8_t consecutive_failures_{0}; + + float declination_{0.0f}; + float heading_offset_{0.0f}; + bool soft_iron_{false}; + CalibrationMode calibration_mode_{CALIBRATION_MODE_YAW}; + BMM150AxisMap mag_axes_; + BMM150AxisMap accel_axes_; + + BMM150Calibration calib_{}; + ESPPreferenceObject pref_; + bool calibrating_{false}; + float cal_min_[3]{}; + float cal_max_[3]{}; + bool tilt_unavailable_logged_{false}; + bool init_retry_logged_{false}; + Trigger calibration_finished_trigger_; int8_t bmm150_initialization(); + void note_failure_(); + void load_calibration_(); + void save_calibration_(); + void finish_calibration_(); + void reset_calibration_(); + void stamp_calibration_context_(BMM150Calibration *out) const; + bool calibration_matches_config_(const BMM150Calibration &c) const; + void apply_axes_(const float in[3], const BMM150AxisMap &map, float out[3]) const; + bool has_accel_ids_() const; + bool read_accel_(float accel[3]) const; + float apply_heading_offsets_(float magnetic_heading) const; + float compute_planar_heading_(float mx, float my) const; + float compute_tilt_heading_(float mx, float my, float mz, const float accel[3]) const; + static float wrap_degrees_(float deg); +}; + +template class CalibrateAction : public Action { + public: + explicit CalibrateAction(BMM150Component *parent) : parent_(parent) {} + void set_duration(uint32_t duration_ms) { this->duration_ms_ = duration_ms; } + void play(const Ts &...x) override { this->parent_->start_calibration(this->duration_ms_); } + + protected: + BMM150Component *parent_; + uint32_t duration_ms_{30000}; }; } // namespace bmm150 } // namespace esphome -#endif diff --git a/components/bmm150/sensor.py b/components/bmm150/sensor.py index ab3a03a3..c3c6d9a4 100644 --- a/components/bmm150/sensor.py +++ b/components/bmm150/sensor.py @@ -1,36 +1,119 @@ +from esphome import automation import esphome.codegen as cg -import esphome.config_validation as cv from esphome.components import i2c, sensor -from esphome.const import CONF_ID, STATE_CLASS_MEASUREMENT +import esphome.config_validation as cv +from esphome.const import ( + CONF_DURATION, + CONF_HEADING, + CONF_ID, + ICON_MAGNET, + ICON_SCREEN_ROTATION, + STATE_CLASS_MEASUREMENT, + UNIT_DEGREES, + UNIT_MICROTESLA, +) CODEOWNERS = ["@eigger"] -DEPENDENCIES = ['i2c'] -CONF_MAG_X = 'magnetic_field_x' -CONF_MAG_Y = 'magnetic_field_y' -CONF_MAG_Z = 'magnetic_field_z' +DEPENDENCIES = ["i2c"] -bmm150_ns = cg.esphome_ns.namespace('bmm150') -BMM150Component = bmm150_ns.class_('BMM150Component', cg.PollingComponent, i2c.I2CDevice) +CONF_MAG_X = "magnetic_field_x" +CONF_MAG_Y = "magnetic_field_y" +CONF_MAG_Z = "magnetic_field_z" +CONF_ACCEL_X_ID = "accel_x_id" +CONF_ACCEL_Y_ID = "accel_y_id" +CONF_ACCEL_Z_ID = "accel_z_id" +CONF_DECLINATION = "declination" +CONF_HEADING_OFFSET = "heading_offset" +CONF_SOFT_IRON = "soft_iron" +CONF_CALIBRATION_MODE = "calibration_mode" +CONF_MAG_AXES = "mag_axes" +CONF_ACCEL_AXES = "accel_axes" +CONF_ON_CALIBRATION_FINISHED = "on_calibration_finished" + +AXIS_MAP = { + "x": (0, 1), + "y": (1, 1), + "z": (2, 1), + "-x": (0, -1), + "-y": (1, -1), + "-z": (2, -1), +} + +bmm150_ns = cg.esphome_ns.namespace("bmm150") +BMM150Component = bmm150_ns.class_("BMM150Component", cg.PollingComponent, i2c.I2CDevice) +CalibrateAction = bmm150_ns.class_("CalibrateAction", automation.Action) +CalibrationMode = bmm150_ns.enum("CalibrationMode") +CALIBRATION_MODES = { + "yaw": CalibrationMode.CALIBRATION_MODE_YAW, + "full": CalibrationMode.CALIBRATION_MODE_FULL, +} + + +def validate_axes(value): + value = cv.ensure_list(cv.one_of(*AXIS_MAP, lower=True))(value) + if len(value) != 3: + raise cv.Invalid("Must specify exactly 3 axes") + indices = sorted(AXIS_MAP[v][0] for v in value) + if indices != [0, 1, 2]: + raise cv.Invalid("Axes must be a permutation of x, y, z (optional leading '-')") + return value + + +def validate_accel(config): + keys = (CONF_ACCEL_X_ID, CONF_ACCEL_Y_ID, CONF_ACCEL_Z_ID) + present = [k in config for k in keys] + if any(present) and not all(present): + raise cv.Invalid("accel_x_id, accel_y_id and accel_z_id must all be set together") + return config + + +def axes_to_args(axes): + args = [] + for name in axes: + src, sign = AXIS_MAP[name] + args.extend((src, sign)) + return args + + +mag_schema = sensor.sensor_schema( + unit_of_measurement=UNIT_MICROTESLA, + icon=ICON_MAGNET, + accuracy_decimals=0, + state_class=STATE_CLASS_MEASUREMENT, +) +heading_schema = sensor.sensor_schema( + unit_of_measurement=UNIT_DEGREES, + icon=ICON_SCREEN_ROTATION, + accuracy_decimals=1, + state_class=STATE_CLASS_MEASUREMENT, +) + +CONFIG_SCHEMA = cv.All( + cv.Schema( + { + cv.GenerateID(): cv.declare_id(BMM150Component), + cv.Optional(CONF_MAG_X): mag_schema, + cv.Optional(CONF_MAG_Y): mag_schema, + cv.Optional(CONF_MAG_Z): mag_schema, + cv.Optional(CONF_HEADING): heading_schema, + cv.Optional(CONF_ACCEL_X_ID): cv.use_id(sensor.Sensor), + cv.Optional(CONF_ACCEL_Y_ID): cv.use_id(sensor.Sensor), + cv.Optional(CONF_ACCEL_Z_ID): cv.use_id(sensor.Sensor), + cv.Optional(CONF_DECLINATION, default=0.0): cv.float_, + cv.Optional(CONF_HEADING_OFFSET, default=0.0): cv.float_, + cv.Optional(CONF_SOFT_IRON, default=False): cv.boolean, + cv.Optional(CONF_CALIBRATION_MODE, default="yaw"): cv.enum(CALIBRATION_MODES, lower=True), + cv.Optional(CONF_MAG_AXES, default=["x", "y", "z"]): validate_axes, + cv.Optional(CONF_ACCEL_AXES, default=["x", "y", "z"]): validate_axes, + cv.Optional(CONF_ON_CALIBRATION_FINISHED): automation.validate_automation(single=True), + } + ) + .extend(cv.polling_component_schema("60s")) + .extend(i2c.i2c_device_schema(0x10)), + validate_accel, +) -CONFIG_SCHEMA = cv.Schema({ - cv.GenerateID(): cv.declare_id(BMM150Component), - cv.Optional(CONF_MAG_X): sensor.sensor_schema( - state_class=STATE_CLASS_MEASUREMENT, - icon='mdi:axis-x-arrow', - accuracy_decimals=0, - ), - cv.Optional(CONF_MAG_Y): sensor.sensor_schema( - state_class=STATE_CLASS_MEASUREMENT, - icon='mdi:axis-y-arrow', - accuracy_decimals=0, - ), - cv.Optional(CONF_MAG_Z): sensor.sensor_schema( - state_class=STATE_CLASS_MEASUREMENT, - icon='mdi:axis-z-arrow', - accuracy_decimals=0, - ), -}).extend(cv.polling_component_schema("60s")).extend(i2c.i2c_device_schema(0x10)) async def to_code(config): var = cg.new_Pvariable(config[CONF_ID]) @@ -38,14 +121,51 @@ async def to_code(config): await i2c.register_i2c_device(var, config) if CONF_MAG_X in config: - conf = config[CONF_MAG_X] - sens = await sensor.new_sensor(conf) + sens = await sensor.new_sensor(config[CONF_MAG_X]) cg.add(var.set_mag_x(sens)) if CONF_MAG_Y in config: - conf = config[CONF_MAG_Y] - sens = await sensor.new_sensor(conf) + sens = await sensor.new_sensor(config[CONF_MAG_Y]) cg.add(var.set_mag_y(sens)) if CONF_MAG_Z in config: - conf = config[CONF_MAG_Z] - sens = await sensor.new_sensor(conf) + sens = await sensor.new_sensor(config[CONF_MAG_Z]) cg.add(var.set_mag_z(sens)) + if CONF_HEADING in config: + sens = await sensor.new_sensor(config[CONF_HEADING]) + cg.add(var.set_heading(sens)) + + if CONF_ACCEL_X_ID in config: + cg.add(var.set_accel_x(await cg.get_variable(config[CONF_ACCEL_X_ID]))) + cg.add(var.set_accel_y(await cg.get_variable(config[CONF_ACCEL_Y_ID]))) + cg.add(var.set_accel_z(await cg.get_variable(config[CONF_ACCEL_Z_ID]))) + + cg.add(var.set_declination(config[CONF_DECLINATION])) + cg.add(var.set_heading_offset(config[CONF_HEADING_OFFSET])) + cg.add(var.set_soft_iron(config[CONF_SOFT_IRON])) + cg.add(var.set_calibration_mode(config[CONF_CALIBRATION_MODE])) + cg.add(var.set_mag_axes(*axes_to_args(config[CONF_MAG_AXES]))) + cg.add(var.set_accel_axes(*axes_to_args(config[CONF_ACCEL_AXES]))) + + if CONF_ON_CALIBRATION_FINISHED in config: + await automation.build_automation( + var.get_calibration_finished_trigger(), + [(cg.bool_, "success")], + config[CONF_ON_CALIBRATION_FINISHED], + ) + + +@automation.register_action( + "bmm150.calibrate", + CalibrateAction, + cv.Schema( + { + cv.GenerateID(): cv.use_id(BMM150Component), + cv.Optional(CONF_DURATION, default="30s"): cv.positive_time_period_milliseconds, + } + ), + synchronous=True, +) +async def bmm150_calibrate_to_code(config, action_id, template_arg, args): + paren = await cg.get_variable(config[CONF_ID]) + var = cg.new_Pvariable(action_id, template_arg, paren) + cg.add(var.set_duration(config[CONF_DURATION])) + return var diff --git a/tests/components/bmm150/common.yaml b/tests/components/bmm150/common.yaml index d6126dac..5f7cab26 100644 --- a/tests/components/bmm150/common.yaml +++ b/tests/components/bmm150/common.yaml @@ -7,18 +7,55 @@ external_components: esphome: name: test-bmm150 +logger: + i2c: sda: ${sda} scl: ${scl} scan: true - + sensor: + - platform: template + id: dummy_accel_x + lambda: "return 0.0f;" + update_interval: 1s + - platform: template + id: dummy_accel_y + lambda: "return 0.0f;" + update_interval: 1s + - platform: template + id: dummy_accel_z + lambda: "return 1.0f;" + update_interval: 1s + - platform: bmm150 + id: bmm150_mag address: 0x10 - update_interval: 30s + update_interval: 1s magnetic_field_x: name: "MAG X" magnetic_field_y: name: "MAG Y" magnetic_field_z: name: "MAG Z" + heading: + name: "Heading" + accel_x_id: dummy_accel_x + accel_y_id: dummy_accel_y + accel_z_id: dummy_accel_z + declination: 0 + heading_offset: 0 + soft_iron: false + calibration_mode: yaw + mag_axes: [x, y, z] + accel_axes: [x, y, z] + on_calibration_finished: + - lambda: |- + ESP_LOGI("test", "calibration success=%d", success); + +interval: + - interval: 24h + then: + - bmm150.calibrate: + id: bmm150_mag + duration: 30s