save lra calibration data to nvs

src/drivers/haptics.cpp

@@ -15,6 +15,7 @@
 #include "assert.h"
 #include "driver/gpio.h"
 #include "driver/i2c.h"
+#include "drivers/nvs.hpp"
 #include "esp_err.h"
 #include "esp_log.h"
 #include "freertos/projdefs.h"
@@ -28,11 +29,11 @@ namespace drivers {
 static constexpr char kTag[] = "haptics";
 static constexpr uint8_t kHapticsAddress = 0x5A;
 
-Haptics::Haptics(const std::variant<ErmMotor, LraMotor>& motor) {
-  // Bring the driver out of standby, and put it into auto-calibration mode.
+Haptics::Haptics(NvsStorage& nvs) {
+  // Bring the driver out of standby, and put it into calibration mode.
   WriteRegister(Register::kMode, 0b00000111);
 
-  if (std::holds_alternative<ErmMotor>(motor)) {
+  if (nvs.HapticMotorIsErm()) {
     ESP_LOGI(kTag, "Setting up ERM motor...");
 
     // Put into ERM Open Loop:
@@ -54,10 +55,8 @@ Haptics::Haptics(const std::variant<ErmMotor, LraMotor>& motor) {
     WriteRegister(Register::kWaveformLibrary,
                   static_cast<uint8_t>(kDefaultErmLibrary));
 
-  } else if (std::holds_alternative<LraMotor>(motor)) {
+  } else {
     ESP_LOGI(kTag, "Setting up LRA motor...");
-    // TODO: Save and restore calibration data instead of recalibrating each
-    // boot.
 
     // Set rated voltage to 1v8; see equation (5) in Section 8.5.2.1.
     WriteRegister(Register::kRatedVoltage, 0x46);
@@ -67,9 +66,6 @@ Haptics::Haptics(const std::variant<ErmMotor, LraMotor>& motor) {
     // FIXME: Could this be higher?
     WriteRegister(Register::kOverdriveClampVoltage, 0x7B);
 
-    // Enable LRA mode, with default brake factor, loop gain, and back-EMF.
-    WriteRegister(Register::kFeedbackControl, 0b10110110);
-
     // Set the drive time for our 235Hz motors. This is a period of 4.2ms, so
     // a drive time of 2.1ms. See Table 24 in section 8.6.21. Leave the other
     // bits in this register with their default values.
@@ -78,17 +74,50 @@ Haptics::Haptics(const std::variant<ErmMotor, LraMotor>& motor) {
     // Ensure closed-loop mode is set.
     WriteRegister(Register::kControl3, 0b10000000);
 
-    // Use a short auto-calibration time. For our application, a longer time
-    // doesn't seem to affect much.
-    WriteRegister(Register::kControl4, 0b00000000);
+    auto calibration = nvs.LraCalibration();
+    if (!calibration) {
+      ESP_LOGI(kTag, "calibrating LRA motor");
+      // Enable LRA mode, with default brake factor, loop gain, and back-EMF.
+      WriteRegister(Register::kFeedbackControl, 0b10110110);
 
-    // Start auto-calibration.
-    WriteRegister(Register::kGo, 1);
+      // Start auto-calibration.
+      WriteRegister(Register::kGo, 1);
+      // Wait for calibration to complete.
+      do {
+        vTaskDelay(1);
+      } while ((ReadRegister(Register::kGo) & 1) > 0);
 
-    // Wait for calibration to complete.
-    do {
-      vTaskDelay(1);
-    } while ((ReadRegister(Register::kGo) & 1) > 0);
+      uint8_t status = ReadRegister(Register::kStatus);
+      if ((status & 0b11111) == 0) {
+        calibration = NvsStorage::LraData{
+            .compensation =
+                ReadRegister(Register::kAutoCalibrationCompensationResult),
+            .back_emf = ReadRegister(Register::kAutoCalibrationBackEmfResult),
+            .gain = static_cast<uint8_t>(
+                ReadRegister(Register::kFeedbackControl) & 0b11),
+        };
+
+        ESP_LOGI(kTag, "lra calibration succeeded: %x%x%x",
+                 calibration->compensation, calibration->back_emf,
+                 calibration->gain);
+
+        nvs.LraCalibration(*calibration);
+      } else {
+        // Not much we can do here! The motor might still buzz okay, so it's
+        // not a fatal issue at least.
+        ESP_LOGE(kTag, "lra calibration failed :(");
+      }
+    } else {
+      ESP_LOGI(kTag, "using stored lra calibration: %x%x%x",
+               calibration->compensation, calibration->back_emf,
+               calibration->gain);
+
+      WriteRegister(Register::kAutoCalibrationCompensationResult,
+                    calibration->compensation);
+      WriteRegister(Register::kAutoCalibrationBackEmfResult,
+                    calibration->back_emf);
+      WriteRegister(Register::kFeedbackControl, 0b10110100 | calibration->gain);
+    }
 
     // Set library; only option is the LRA one for, well, LRA motors.
     WriteRegister(Register::kWaveformLibrary,
@@ -97,6 +126,9 @@ Haptics::Haptics(const std::variant<ErmMotor, LraMotor>& motor) {
 
   // Set mode (internal trigger, on writing 1 to Go register)
   WriteRegister(Register::kMode, static_cast<uint8_t>(Mode::kInternalTrigger));
+
+  // Set up a default effect (sequence of one effect)
+  SetWaveformEffect(kStartupEffect);
 }
 
 Haptics::~Haptics() {}

src/drivers/include/drivers/haptics.hpp

@@ -14,23 +14,15 @@
 #include <string>
 #include <variant>
 
-namespace drivers {
+#include "drivers/nvs.hpp"
 
-typedef std::monostate ErmMotor;
-struct LraMotor {
-  // TODO: fill out with calibration data from
-  // https://www.ti.com/lit/ds/symlink/drv2605l.pdf
-  bool hi;
-};
+namespace drivers {
 
 class Haptics {
  public:
-  static auto Create(const std::variant<ErmMotor, LraMotor>& motor)
-      -> Haptics* {
-    return new Haptics(motor);
-  }
+  static auto Create(NvsStorage& nvs) -> Haptics* { return new Haptics(nvs); }
 
-  Haptics(const std::variant<ErmMotor, LraMotor>& motor);
+  Haptics(NvsStorage& nvs);
   ~Haptics();
 
   // Not copyable or movable.

src/drivers/include/drivers/nvs.hpp

@@ -78,7 +78,17 @@ class NvsStorage {
 
   auto HapticMotorIsErm() -> bool;
   auto HapticMotorIsErm(bool) -> void;
-  // /Hardware Compatibility
+
+  struct LraData {
+    uint8_t compensation;
+    uint8_t back_emf;
+    uint8_t gain;
+
+    bool operator==(const LraData&) const = default;
+  };
+
+  auto LraCalibration() -> std::optional<LraData>;
+  auto LraCalibration(const LraData&) -> void;
 
   auto PreferredBluetoothDevice() -> std::optional<bluetooth::MacAndName>;
   auto PreferredBluetoothDevice(std::optional<bluetooth::MacAndName>) -> void;
@@ -135,6 +145,7 @@ class NvsStorage {
   Setting<uint16_t> display_cols_;
   Setting<uint16_t> display_rows_;
   Setting<uint8_t> haptic_motor_type_;
+  Setting<LraData> lra_calibration_;
 
   Setting<uint8_t> brightness_;
   Setting<uint8_t> sensitivity_;

src/drivers/nvs.cpp

@@ -37,10 +37,11 @@ static constexpr char kKeyLockPolarity[] = "lockpol";
 static constexpr char kKeyDisplayCols[] = "dispcols";
 static constexpr char kKeyDisplayRows[] = "disprows";
 static constexpr char kKeyHapticMotorType[] = "hapticmtype";
+static constexpr char kKeyLraCalibration[] = "lra_cali";
 static constexpr char kKeyDbAutoIndex[] = "dbautoindex";
 
-static auto nvs_get_string(nvs_handle_t nvs,
-                           const char* key) -> std::optional<std::string> {
+static auto nvs_get_string(nvs_handle_t nvs, const char* key)
+    -> std::optional<std::string> {
   size_t len = 0;
   if (nvs_get_blob(nvs, key, NULL, &len) != ESP_OK) {
     return {};
@@ -128,6 +129,39 @@ auto Setting<bluetooth::MacAndName>::store(nvs_handle_t nvs,
   nvs_set_blob(nvs, name_, encoded.data(), encoded.size());
 }
 
+template <>
+auto Setting<NvsStorage::LraData>::load(nvs_handle_t nvs)
+    -> std::optional<NvsStorage::LraData> {
+  auto raw = nvs_get_string(nvs, name_);
+  if (!raw) {
+    return {};
+  }
+  auto [parsed, unused, err] = cppbor::parseWithViews(
+      reinterpret_cast<const uint8_t*>(raw->data()), raw->size());
+  if (parsed->type() != cppbor::ARRAY) {
+    return {};
+  }
+  auto arr = parsed->asArray();
+  NvsStorage::LraData data{
+      .compensation = static_cast<uint8_t>(arr->get(0)->asUint()->value()),
+      .back_emf = static_cast<uint8_t>(arr->get(1)->asUint()->value()),
+      .gain = static_cast<uint8_t>(arr->get(2)->asUint()->value()),
+  };
+  return data;
+}
+
+template <>
+auto Setting<NvsStorage::LraData>::store(nvs_handle_t nvs,
+                                         NvsStorage::LraData v) -> void {
+  cppbor::Array cbor{
+      cppbor::Uint{v.compensation},
+      cppbor::Uint{v.back_emf},
+      cppbor::Uint{v.gain},
+  };
+  auto encoded = cbor.encode();
+  nvs_set_blob(nvs, name_, encoded.data(), encoded.size());
+}
+
 auto NvsStorage::OpenSync() -> NvsStorage* {
   esp_err_t err = nvs_flash_init();
   if (err == ESP_ERR_NVS_NO_FREE_PAGES) {
@@ -165,6 +199,7 @@ NvsStorage::NvsStorage(nvs_handle_t handle)
       display_cols_(kKeyDisplayCols),
       display_rows_(kKeyDisplayRows),
       haptic_motor_type_(kKeyHapticMotorType),
+      lra_calibration_(kKeyLraCalibration),
       brightness_(kKeyBrightness),
       sensitivity_(kKeyScrollSensitivity),
       amp_max_vol_(kKeyAmpMaxVolume),
@@ -187,7 +222,9 @@ auto NvsStorage::Read() -> void {
   lock_polarity_.read(handle_);
   display_cols_.read(handle_);
   display_rows_.read(handle_);
-  haptic_motor_type_.read(handle_), brightness_.read(handle_);
+  haptic_motor_type_.read(handle_);
+  lra_calibration_.read(handle_);
+  brightness_.read(handle_);
   sensitivity_.read(handle_);
   amp_max_vol_.read(handle_);
   amp_cur_vol_.read(handle_);
@@ -204,7 +241,9 @@ auto NvsStorage::Write() -> bool {
   lock_polarity_.write(handle_);
   display_cols_.write(handle_);
   display_rows_.write(handle_);
-  haptic_motor_type_.write(handle_), brightness_.write(handle_);
+  haptic_motor_type_.write(handle_);
+  lra_calibration_.write(handle_);
+  brightness_.write(handle_);
   sensitivity_.write(handle_);
   amp_max_vol_.write(handle_);
   amp_cur_vol_.write(handle_);
@@ -252,6 +291,16 @@ auto NvsStorage::HapticMotorIsErm(bool p) -> void {
   haptic_motor_type_.set(p);
 }
 
+auto NvsStorage::LraCalibration() -> std::optional<LraData> {
+  std::lock_guard<std::mutex> lock{mutex_};
+  return lra_calibration_.get();
+}
+
+auto NvsStorage::LraCalibration(const LraData& d) -> void {
+  std::lock_guard<std::mutex> lock{mutex_};
+  lra_calibration_.set(d);
+}
+
 auto NvsStorage::DisplaySize()
     -> std::pair<std::optional<uint16_t>, std::optional<uint16_t>> {
   std::lock_guard<std::mutex> lock{mutex_};
@@ -285,8 +334,8 @@ auto NvsStorage::BluetoothVolume(const bluetooth::mac_addr_t& mac) -> uint8_t {
   return bt_volumes_.Get(mac).value_or(50);
 }
 
-auto NvsStorage::BluetoothVolume(const bluetooth::mac_addr_t& mac,
-                                 uint8_t vol) -> void {
+auto NvsStorage::BluetoothVolume(const bluetooth::mac_addr_t& mac, uint8_t vol)
+    -> void {
   std::lock_guard<std::mutex> lock{mutex_};
   bt_volumes_dirty_ = true;
   bt_volumes_.Put(mac, vol);

src/tangara/system_fsm/booting.cpp

@@ -4,12 +4,9 @@
  * SPDX-License-Identifier: GPL-3.0-only
  */
 
-#include "collation.hpp"
-#include "drivers/haptics.hpp"
-#include "drivers/spiffs.hpp"
 #include "system_fsm/system_fsm.hpp"
 
-#include <stdint.h>
+#include <cstdint>
 #include <memory>
 
 #include "assert.h"
@@ -23,16 +20,19 @@
 #include "audio/audio_fsm.hpp"
 #include "audio/track_queue.hpp"
 #include "battery/battery.hpp"
+#include "collation.hpp"
 #include "database/tag_parser.hpp"
 #include "drivers/adc.hpp"
 #include "drivers/bluetooth.hpp"
 #include "drivers/bluetooth_types.hpp"
 #include "drivers/display_init.hpp"
 #include "drivers/gpios.hpp"
+#include "drivers/haptics.hpp"
 #include "drivers/i2c.hpp"
 #include "drivers/nvs.hpp"
 #include "drivers/samd.hpp"
 #include "drivers/spi.hpp"
+#include "drivers/spiffs.hpp"
 #include "drivers/touchwheel.hpp"
 #include "events/event_queue.hpp"
 #include "system_fsm/service_locator.hpp"
@@ -89,11 +89,7 @@ auto Booting::entry() -> void {
   sServices->samd(std::unique_ptr<drivers::Samd>(drivers::Samd::Create()));
   sServices->touchwheel(
       std::unique_ptr<drivers::TouchWheel>{drivers::TouchWheel::Create()});
-  sServices->haptics(std::make_unique<drivers::Haptics>(
-      sServices->nvs().HapticMotorIsErm()
-          ? std::variant<drivers::ErmMotor, drivers::LraMotor>(
-                drivers::ErmMotor())
-          : drivers::LraMotor()));
+  sServices->haptics(std::make_unique<drivers::Haptics>(sServices->nvs()));
 
   auto adc = drivers::AdcBattery::Create();
   sServices->battery(std::make_unique<battery::Battery>(