From 7c075cf5b776feaed2065f936dff0c176635b89d Mon Sep 17 00:00:00 2001 From: Robin Howard Date: Fri, 19 Apr 2024 16:03:35 +1000 Subject: [PATCH] Adds LRA haptic support (open-loop only for now). --- src/drivers/haptics.cpp | 67 ++++++++++++++++++++++++--------- src/drivers/include/haptics.hpp | 24 +++++++++--- src/drivers/include/nvs.hpp | 6 +++ src/drivers/nvs.cpp | 14 +++++++ src/system_fsm/booting.cpp | 10 ++++- 5 files changed, 97 insertions(+), 24 deletions(-) diff --git a/src/drivers/haptics.cpp b/src/drivers/haptics.cpp index ccc9e599..f7b18086 100644 --- a/src/drivers/haptics.cpp +++ b/src/drivers/haptics.cpp @@ -28,24 +28,55 @@ namespace drivers { static constexpr char kTag[] = "haptics"; static constexpr uint8_t kHapticsAddress = 0x5A; -Haptics::Haptics() { +Haptics::Haptics(const std::variant& motor) { PowerUp(); - // Put into ERM Open Loop: - // (§8.5.4.1 Programming for ERM Open-Loop Operation) - // - Turn off N_ERM_LRA first - WriteRegister(Register::kControl1, - static_cast(RegisterDefaults::kControl1) & - (~ControlMask::kNErmLra)); - // - Turn on ERM_OPEN_LOOP - WriteRegister(Register::kControl3, - static_cast(RegisterDefaults::kControl3) | - ControlMask::kErmOpenLoop); - - // Set library - // TODO(robin): try the other libraries and test response. C is marginal, D - // too much? - WriteRegister(Register::kWaveformLibrary, static_cast(kDefaultLibrary)); + // TODO: Break out into helper functions + // TODO: Use LRA closed loop instead, loading in calibration data from NVS, or + // running calibration + storing the result first if necessary. + + if (std::holds_alternative(motor)) { + ESP_LOGI(kTag, "Setting up ERM motor..."); + + // Put into ERM Open Loop: + // (§8.5.4.1 Programming for ERM Open-Loop Operation) + + // - Turn off N_ERM_LRA first + WriteRegister(Register::kFeedbackControl, + static_cast(RegisterDefaults::kFeedbackControl) & + (~ControlMask::kNErmLra)); + + // - Turn on ERM_OPEN_LOOP + WriteRegister(Register::kControl3, + static_cast(RegisterDefaults::kControl3) | + ControlMask::kErmOpenLoop); + + // Set library + // TODO(robin): try the other libraries and test response. C is marginal, D + // too much? + WriteRegister(Register::kWaveformLibrary, static_cast(kDefaultErmLibrary)); + + } else if (std::holds_alternative(motor)) { + ESP_LOGI(kTag, "Setting up LRA motor..."); + // TODO: + // auto lraInit = std::get(motor); + + // Put into LRA Open Loop: + // (§8.5.4.1 Programming for LRA Open-Loop Operation) + + // - Turn on N_ERM_LRA first + WriteRegister(Register::kFeedbackControl, + static_cast(RegisterDefaults::kFeedbackControl) & + (ControlMask::kNErmLra)); + + // - Turn on LRA_OPEN_LOOP + WriteRegister(Register::kControl3, + static_cast(RegisterDefaults::kControl3) | + ControlMask::kLraOpenLoop); + + // Set library; only option is the LRA one for, well, LRA motors. + WriteRegister(Register::kWaveformLibrary, static_cast(Library::LRA)); + } // Set mode (internal trigger, on writing 1 to Go register) WriteRegister(Register::kMode, static_cast(Mode::kInternalTrigger)); @@ -94,13 +125,13 @@ auto Haptics::SetWaveformEffect(Effect effect) -> void { auto Haptics::TourEffects() -> void { - TourEffects(Effect::kFirst, Effect::kLast, kDefaultLibrary); + TourEffects(Effect::kFirst, Effect::kLast, kDefaultErmLibrary); } auto Haptics::TourEffects(Library lib) -> void { TourEffects(Effect::kFirst, Effect::kLast, lib); } auto Haptics::TourEffects(Effect from, Effect to) -> void { - TourEffects(from, to, kDefaultLibrary); + TourEffects(from, to, kDefaultErmLibrary); } auto Haptics::TourEffects(Effect from, Effect to, Library lib) -> void { ESP_LOGI(kTag, "With library #%u...", static_cast(lib)); diff --git a/src/drivers/include/haptics.hpp b/src/drivers/include/haptics.hpp index cf970977..940c3c6d 100644 --- a/src/drivers/include/haptics.hpp +++ b/src/drivers/include/haptics.hpp @@ -11,13 +11,24 @@ #include #include #include +#include namespace drivers { +typedef std::monostate ErmMotor; +struct LraMotor { + // TODO: fill out with calibration data from https://www.ti.com/lit/ds/symlink/drv2605l.pdf + bool hi; +}; + class Haptics { public: - static auto Create() -> Haptics* { return new Haptics(); } - Haptics(); + static auto Create(const std::variant& motor) + -> Haptics* { + return new Haptics(motor); + } + + Haptics(const std::variant& motor); ~Haptics(); // Not copyable or movable. @@ -169,11 +180,12 @@ class Haptics { B = 2, // 3V, Rise: 40-60ms, Brake: 5-15ms C = 3, // 3V, Rise: 60-80ms, Brake: 10-20ms D = 4, // 3V, Rise: 100-140ms, Brake: 15-25ms - E = 5 // 3V, Rise: >140ms, Brake: >30ms - // 6 is LRA-only, 7 is 4.5V+ + E = 5, // 3V, Rise: >140ms, Brake: >30ms + LRA = 6 + // 7 is 4.5V+ }; - static constexpr Library kDefaultLibrary = Library::C; + static constexpr Library kDefaultErmLibrary = Library::C; auto PowerDown() -> void; auto Reset() -> void; @@ -216,8 +228,10 @@ class Haptics { struct ControlMask { // FeedbackControl static constexpr uint8_t kNErmLra = 0b10000000; + // Control3 static constexpr uint8_t kErmOpenLoop = 0b00100000; + static constexpr uint8_t kLraOpenLoop = 0b00000001; }; // §8.6 Register Map diff --git a/src/drivers/include/nvs.hpp b/src/drivers/include/nvs.hpp index 25396622..7c74ceb0 100644 --- a/src/drivers/include/nvs.hpp +++ b/src/drivers/include/nvs.hpp @@ -68,6 +68,7 @@ class NvsStorage { auto Read() -> void; auto Write() -> bool; + // Hardware Compatibility auto LockPolarity() -> bool; auto LockPolarity(bool) -> void; @@ -76,6 +77,10 @@ class NvsStorage { auto DisplaySize(std::pair, std::optional>) -> void; + auto HapticMotorIsErm() -> bool; + auto HapticMotorIsErm(bool) -> void; + // /Hardware Compatibility + auto PreferredBluetoothDevice() -> std::optional; auto PreferredBluetoothDevice(std::optional) -> void; @@ -130,6 +135,7 @@ class NvsStorage { Setting lock_polarity_; Setting display_cols_; Setting display_rows_; + Setting haptic_motor_type_; Setting brightness_; Setting sensitivity_; diff --git a/src/drivers/nvs.cpp b/src/drivers/nvs.cpp index fbdb5286..c8befe48 100644 --- a/src/drivers/nvs.cpp +++ b/src/drivers/nvs.cpp @@ -39,6 +39,7 @@ static constexpr char kKeyScrollSensitivity[] = "scroll"; static constexpr char kKeyLockPolarity[] = "lockpol"; static constexpr char kKeyDisplayCols[] = "dispcols"; static constexpr char kKeyDisplayRows[] = "disprows"; +static constexpr char kKeyHapticMotorType[] = "hapticmtype"; static constexpr char kKeyDbAutoIndex[] = "dbautoindex"; static auto nvs_get_string(nvs_handle_t nvs, const char* key) @@ -166,6 +167,7 @@ NvsStorage::NvsStorage(nvs_handle_t handle) lock_polarity_(kKeyLockPolarity), display_cols_(kKeyDisplayCols), display_rows_(kKeyDisplayRows), + haptic_motor_type_(kKeyHapticMotorType), brightness_(kKeyBrightness), sensitivity_(kKeyScrollSensitivity), amp_max_vol_(kKeyAmpMaxVolume), @@ -188,6 +190,7 @@ auto NvsStorage::Read() -> void { lock_polarity_.read(handle_); display_cols_.read(handle_); display_rows_.read(handle_); + haptic_motor_type_.read(handle_), brightness_.read(handle_); sensitivity_.read(handle_); amp_max_vol_.read(handle_); @@ -205,6 +208,7 @@ auto NvsStorage::Write() -> bool { lock_polarity_.write(handle_); display_cols_.write(handle_); display_rows_.write(handle_); + haptic_motor_type_.write(handle_), brightness_.write(handle_); sensitivity_.write(handle_); amp_max_vol_.write(handle_); @@ -243,6 +247,16 @@ auto NvsStorage::LockPolarity(bool p) -> void { lock_polarity_.set(p); } +auto NvsStorage::HapticMotorIsErm() -> bool { + std::lock_guard lock{mutex_}; + return haptic_motor_type_.get().value_or(0) > 0; +} + +auto NvsStorage::HapticMotorIsErm(bool p) -> void { + std::lock_guard lock{mutex_}; + haptic_motor_type_.set(p); +} + auto NvsStorage::DisplaySize() -> std::pair, std::optional> { std::lock_guard lock{mutex_}; diff --git a/src/system_fsm/booting.cpp b/src/system_fsm/booting.cpp index bd394428..a89f35d9 100644 --- a/src/system_fsm/booting.cpp +++ b/src/system_fsm/booting.cpp @@ -62,6 +62,10 @@ auto Booting::entry() -> void { sServices->nvs( std::unique_ptr(drivers::NvsStorage::OpenSync())); + // HACK: tell the unit that it has an ERM motor (we will likely default to + // LRAs in future, but all the current units in the field use ERMs.) + sServices->nvs().HapticMotorIsErm(true); + // HACK: fix up the switch polarity on newer dev units // sServices->nvs().LockPolarity(false); @@ -85,7 +89,11 @@ 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->haptics(std::make_unique( + sServices->nvs().HapticMotorIsErm() + ? std::variant( + drivers::ErmMotor()) + : drivers::LraMotor())); auto adc = drivers::AdcBattery::Create(); sServices->battery(std::make_unique(