esphome/esphome/components/motion/motion_component.h

#pragma once

#include "esphome/core/automation.h"
#include "esphome/core/component.h"
#include "esphome/core/helpers.h"
#include "esphome/core/preferences.h"
#include <array>
#include <cmath>
#include <numbers>  // required for generated lambda code

namespace esphome::motion {

// ---Data class

struct MotionData {
  float acceleration[3]{NAN, NAN, NAN};
  float angular_rate[3]{NAN, NAN, NAN};
  // TODO - compass
};

// indices into data arrays
static constexpr uint8_t X_AXIS = 0;
static constexpr uint8_t Y_AXIS = 1;
static constexpr uint8_t Z_AXIS = 2;

/// Compute the device's in-plane orientation from the gravity vector.
///
/// Returns NAN when the device is flat (lying face up or face down), i.e. when the
/// horizontal component of gravity, normalised by the total acceleration, is below
/// `flat_threshold` (the sine of the minimum tilt angle). Otherwise returns the
/// rotation snapped to the nearest of 0, 90, 180 or 270 degrees, derived from the
/// direction of the horizontal gravity component.
inline float orientation_degrees(const MotionData &data, float flat_threshold) {
  float ax = data.acceleration[X_AXIS];
  float ay = data.acceleration[Y_AXIS];
  float az = data.acceleration[Z_AXIS];
  if (std::isnan(ax) || std::isnan(ay) || std::isnan(az))
    return NAN;
  float mag = std::sqrt(ax * ax + ay * ay + az * az);
  if (mag < 0.1f)
    return NAN;
  // Horizontal component of gravity; near zero when the device lies flat.
  float h = std::sqrt(ax * ax + ay * ay);
  if (h / mag < flat_threshold)
    return NAN;
  // Direction of the horizontal component, snapped to the nearest 90°.
  float angle = std::atan2(ay, ax) * (180.0f / std::numbers::pi_v<float>);
  int quadrant = static_cast<int>(std::lround(angle / 90.0f));
  quadrant = ((quadrant % 4) + 4) % 4;  // normalise to 0..3
  return quadrant * 90.0f;
}

// Persisted calibration. `base_hash` ties the stored matrix to the build-time
// (axis_map / transform_matrix) base; if the base changes the saved calibration
// is ignored. Stored under a stable, ID-derived key so it overwrites in place.
struct CalibrationPref {
  uint32_t base_hash;
  float matrix[9];
} PACKED;

// Main component class
class MotionComponent : public PollingComponent {
 public:
  // Lifecycle
  void setup() override;
  void update() override;
  void dump_config() override;
  float get_setup_priority() const override { return setup_priority::DATA; }

  void set_matrix(const std::array<float, 9> &m) {
    memcpy(this->base_matrix_, m.data(), sizeof(this->base_matrix_));
    memcpy(this->matrix_, m.data(), sizeof(this->matrix_));
  }
  void set_calibration_key(uint32_t key) { this->pref_key_ = key; }

  /// Calibrate the matrix so the current reading maps to [0, 0, 1] (device flat).
  bool calibrate_level();
  /// Assuming Y-axis rotation only, correct the heading so X/Y align correctly.
  bool calibrate_heading();
  /// Save the current matrix to NVS.
  bool save_calibration();
  /// Restore the build-time (axis_map / transform_matrix) base, discarding calibration,
  /// and persist that base when `save` is set.
  void clear_calibration(bool save = false);

  template<typename F> void add_listener(F &&cb) { this->motion_data_callback_.add(std::forward<F>(cb)); }

 protected:
  // platforms must implement this method to update raw data.
  virtual bool update_data(MotionData &data) = 0;

  // for mapping axes
  float matrix_[9]{
      1, 0, 0, 0, 1, 0, 0, 0, 1,
  };
  // build-time base (axis_map / transform_matrix); used to detect config changes
  // and to restore on clear_calibration().
  float base_matrix_[9]{
      1, 0, 0, 0, 1, 0, 0, 0, 1,
  };

  void map_axes_(float output[3], const float input[3]) const {
    output[0] = input[X_AXIS] * this->matrix_[0] + input[Y_AXIS] * this->matrix_[1] + input[Z_AXIS] * this->matrix_[2];
    output[1] = input[X_AXIS] * this->matrix_[3] + input[Y_AXIS] * this->matrix_[4] + input[Z_AXIS] * this->matrix_[5];
    output[2] = input[X_AXIS] * this->matrix_[6] + input[Y_AXIS] * this->matrix_[7] + input[Z_AXIS] * this->matrix_[8];
  }

  LazyCallbackManager<void(MotionData const &)> motion_data_callback_{};
  uint32_t pref_key_{0};
  uint32_t base_hash_{0};  // hash of base_matrix_, captured in setup()
  ESPPreferenceObject pref_{};
};

// --- Actions ---

template<typename... Ts> class CalibrateLevelAction final : public Action<Ts...> {
 public:
  explicit CalibrateLevelAction(MotionComponent *parent) : parent_(parent) {}
  void set_save(bool save) { this->save_ = save; }
  Trigger<> *get_success_trigger() { return &this->success_trigger_; }
  Trigger<> *get_error_trigger() { return &this->error_trigger_; }

 protected:
  void play(const Ts &...) override {
    if (this->parent_->calibrate_level()) {
      // if not saving, calibration success is enough. If save required only report success after that succeeds too.
      if (!this->save_ || this->parent_->save_calibration()) {
        this->success_trigger_.trigger();
        return;
      }
    }
    this->error_trigger_.trigger();
  }

  MotionComponent *parent_;
  Trigger<> success_trigger_;
  Trigger<> error_trigger_;
  bool save_{false};
};

template<typename... Ts> class CalibrateHeadingAction final : public Action<Ts...> {
 public:
  explicit CalibrateHeadingAction(MotionComponent *parent) : parent_(parent) {}
  void set_save(bool save) { this->save_ = save; }
  Trigger<> *get_success_trigger() { return &this->success_trigger_; }
  Trigger<> *get_error_trigger() { return &this->error_trigger_; }

 protected:
  void play(const Ts &...) override {
    if (this->parent_->calibrate_heading()) {
      // if not saving, calibration success is enough. If save required only report success after that succeeds too.
      if (!this->save_ || this->parent_->save_calibration()) {
        this->success_trigger_.trigger();
        return;
      }
    }
    this->error_trigger_.trigger();
  }

  MotionComponent *parent_;
  Trigger<> success_trigger_;
  Trigger<> error_trigger_;
  bool save_{false};
};

}  // namespace esphome::motion