From 00d6c2eca20571f1a53400f4f465c8dd7af557db Mon Sep 17 00:00:00 2001 From: jacqueline Date: Tue, 14 May 2024 15:21:21 +1000 Subject: [PATCH] save lra calibration data to nvs --- src/drivers/haptics.cpp | 72 ++++++++++++++++++------- src/drivers/include/drivers/haptics.hpp | 16 ++---- src/drivers/include/drivers/nvs.hpp | 13 ++++- src/drivers/nvs.cpp | 61 ++++++++++++++++++--- src/tangara/system_fsm/booting.cpp | 14 ++--- 5 files changed, 128 insertions(+), 48 deletions(-) diff --git a/src/drivers/haptics.cpp b/src/drivers/haptics.cpp index b2e38634..8e8e5fed 100644 --- a/src/drivers/haptics.cpp +++ b/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& 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(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& motor) { WriteRegister(Register::kWaveformLibrary, static_cast(kDefaultErmLibrary)); - } else if (std::holds_alternative(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& 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& 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); - - // Start auto-calibration. - WriteRegister(Register::kGo, 1); - - // Wait for calibration to complete. - do { - vTaskDelay(1); - } while ((ReadRegister(Register::kGo) & 1) > 0); + 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); + // 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( + 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& motor) { // Set mode (internal trigger, on writing 1 to Go register) WriteRegister(Register::kMode, static_cast(Mode::kInternalTrigger)); + + // Set up a default effect (sequence of one effect) + SetWaveformEffect(kStartupEffect); } Haptics::~Haptics() {} diff --git a/src/drivers/include/drivers/haptics.hpp b/src/drivers/include/drivers/haptics.hpp index a9384fa4..e4666fad 100644 --- a/src/drivers/include/drivers/haptics.hpp +++ b/src/drivers/include/drivers/haptics.hpp @@ -14,23 +14,15 @@ #include #include -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& motor) - -> Haptics* { - return new Haptics(motor); - } + static auto Create(NvsStorage& nvs) -> Haptics* { return new Haptics(nvs); } - Haptics(const std::variant& motor); + Haptics(NvsStorage& nvs); ~Haptics(); // Not copyable or movable. diff --git a/src/drivers/include/drivers/nvs.hpp b/src/drivers/include/drivers/nvs.hpp index 83bb8097..88dd5ae0 100644 --- a/src/drivers/include/drivers/nvs.hpp +++ b/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; + auto LraCalibration(const LraData&) -> void; auto PreferredBluetoothDevice() -> std::optional; auto PreferredBluetoothDevice(std::optional) -> void; @@ -135,6 +145,7 @@ class NvsStorage { Setting display_cols_; Setting display_rows_; Setting haptic_motor_type_; + Setting lra_calibration_; Setting brightness_; Setting sensitivity_; diff --git a/src/drivers/nvs.cpp b/src/drivers/nvs.cpp index 1389fd0d..5c7d2218 100644 --- a/src/drivers/nvs.cpp +++ b/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 { +static auto nvs_get_string(nvs_handle_t nvs, const char* key) + -> std::optional { size_t len = 0; if (nvs_get_blob(nvs, key, NULL, &len) != ESP_OK) { return {}; @@ -128,6 +129,39 @@ auto Setting::store(nvs_handle_t nvs, nvs_set_blob(nvs, name_, encoded.data(), encoded.size()); } +template <> +auto Setting::load(nvs_handle_t nvs) + -> std::optional { + auto raw = nvs_get_string(nvs, name_); + if (!raw) { + return {}; + } + auto [parsed, unused, err] = cppbor::parseWithViews( + reinterpret_cast(raw->data()), raw->size()); + if (parsed->type() != cppbor::ARRAY) { + return {}; + } + auto arr = parsed->asArray(); + NvsStorage::LraData data{ + .compensation = static_cast(arr->get(0)->asUint()->value()), + .back_emf = static_cast(arr->get(1)->asUint()->value()), + .gain = static_cast(arr->get(2)->asUint()->value()), + }; + return data; +} + +template <> +auto Setting::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 { + std::lock_guard lock{mutex_}; + return lra_calibration_.get(); +} + +auto NvsStorage::LraCalibration(const LraData& d) -> void { + std::lock_guard lock{mutex_}; + lra_calibration_.set(d); +} + auto NvsStorage::DisplaySize() -> std::pair, std::optional> { std::lock_guard 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 lock{mutex_}; bt_volumes_dirty_ = true; bt_volumes_.Put(mac, vol); diff --git a/src/tangara/system_fsm/booting.cpp b/src/tangara/system_fsm/booting.cpp index 09d9de80..22a0fcac 100644 --- a/src/tangara/system_fsm/booting.cpp +++ b/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 +#include #include #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::Create())); sServices->touchwheel( std::unique_ptr{drivers::TouchWheel::Create()}); - sServices->haptics(std::make_unique( - sServices->nvs().HapticMotorIsErm() - ? std::variant( - drivers::ErmMotor()) - : drivers::LraMotor())); + sServices->haptics(std::make_unique(sServices->nvs())); auto adc = drivers::AdcBattery::Create(); sServices->battery(std::make_unique(