diff --git a/.github/workflows/upload_components.yml b/.github/workflows/upload_components.yml index 49c56c340d..05453dc8e6 100755 --- a/.github/workflows/upload_components.yml +++ b/.github/workflows/upload_components.yml @@ -31,9 +31,14 @@ jobs: # esp32-p4-nano), which depend on espp/ethernet, so it is # uploaded to the registry first. # + # Note: magnetic_encoder is intentionally listed ahead of as5600 and + # mt6701, which depend on espp/magnetic_encoder, so it is + # uploaded to the registry first. + # # Note: comments are not allowed in the "components" list, so please # do not add any comments here. components: | + components/magnetic_encoder components/adc components/adrc components/ads1x15 diff --git a/components/as5600/CMakeLists.txt b/components/as5600/CMakeLists.txt index 325405646b..e1ec22a324 100644 --- a/components/as5600/CMakeLists.txt +++ b/components/as5600/CMakeLists.txt @@ -1,4 +1,4 @@ idf_component_register( INCLUDE_DIRS "include" - REQUIRES "base_peripheral" "timer" "task" + REQUIRES "magnetic_encoder" "base_peripheral" "timer" ) diff --git a/components/as5600/Kconfig b/components/as5600/Kconfig index 3bf896823f..5f10caebf7 100644 --- a/components/as5600/Kconfig +++ b/components/as5600/Kconfig @@ -10,12 +10,15 @@ menu "AS5600 Configuration" the velocity will be set to 0. This helps filter out noise and small jitter in the encoder readings. - config AS5600_USE_TIMER - bool "Use high resolution timer instead of task" + config AS5600_USE_HIGH_RESOLUTION_TIMER + bool "Use high resolution timer instead of software timer" default y help - Use the high resolution timer instead of a FreeRTOS task for - periodic updates. The timer is more precise and has lower overhead. - Disable this if you prefer to use a task-based implementation. + Drive the periodic encoder update with an esp_timer-backed + HighResolutionTimer (microsecond resolution). This is the right + choice for sub-millisecond update periods. Disable this to instead + use an espp::Timer, which schedules against an absolute wake-up time + (drift-free and in phase) but is limited to the FreeRTOS tick + resolution. endmenu diff --git a/components/as5600/README.md b/components/as5600/README.md index ebbeb74c27..23389a376d 100644 --- a/components/as5600/README.md +++ b/components/as5600/README.md @@ -13,9 +13,11 @@ measure * Accumulated degrees (since the component was created) * Speed (rotations per minute / RPM) -It does so by spawning a task which periodically reads the magnetic encoder, +It does so by spawning a timer which periodically reads the magnetic encoder, updates the accumulator, and computes the velocity. The component can be -configured to optionally filter the velocity. +configured to optionally filter the velocity. The timer can be either a +`espp::HighResolutionTimer` (default) or an `espp::Timer`, selected via KConfig / +menuconfig. The periodicity / update rate of the encoder can be configured at time of creation. diff --git a/components/as5600/example/README.md b/components/as5600/example/README.md index b2e7d898b9..7416abc300 100644 --- a/components/as5600/example/README.md +++ b/components/as5600/example/README.md @@ -9,8 +9,9 @@ ESP32-S3. If you wish to use a different chip / board, changes those and rebuild This example shows the use of the `As5600` component to communicate with an AS5600 I2C magnetic encoder chip. -It uses the `task` component to periodically read the raw count, position, and -velocity of the encoder, the `filters` component (specifically the +It uses the `task` component to periodically poll and print the raw count, +position, and velocity of the encoder (the `As5600` itself maintains that state +with its own internal timer), the `filters` component (specifically the `espp::ButterworthFilter` class) to filter the raw values from the sensor, and the `format` component to print the data to the console in CSV format. diff --git a/components/as5600/idf_component.yml b/components/as5600/idf_component.yml index aa8342853c..44cfb0cf35 100644 --- a/components/as5600/idf_component.yml +++ b/components/as5600/idf_component.yml @@ -18,6 +18,6 @@ tags: dependencies: idf: version: '>=5.0' + espp/magnetic_encoder: '>=1.0' espp/base_peripheral: '>=1.0' - espp/task: '>=1.0' espp/timer: '>=1.0' diff --git a/components/as5600/include/as5600.hpp b/components/as5600/include/as5600.hpp index f735366608..7122558c42 100644 --- a/components/as5600/include/as5600.hpp +++ b/components/as5600/include/as5600.hpp @@ -1,16 +1,21 @@ #pragma once #include -#include -#include +#include #include -#include "base_peripheral.hpp" -#include "high_resolution_timer.hpp" -#include "task.hpp" +#include "magnetic_encoder_base.hpp" namespace espp { +/// @brief Whether the As5600 drives its update loop with a HighResolutionTimer +/// (true) or an espp::Timer (false). Selected by Kconfig / menuconfig. +#if defined(CONFIG_AS5600_USE_HIGH_RESOLUTION_TIMER) +inline constexpr bool as5600_use_high_resolution_timer = true; +#else +inline constexpr bool as5600_use_high_resolution_timer = false; +#endif + /** * @brief Class for position and velocity measurement using a AS5600 magnetic * encoder. This class starts its own measurement task at the specified @@ -20,16 +25,19 @@ namespace espp { * https://ams.com/documents/20143/36005/AS5600_DS000365_5-00.pdf/649ee61c-8f9a-20df-9e10-43173a3eb323 * * This component can be configured to automatically update within its own - * timer/task (timer is default, and can be changed via KConfig / menuconfig), - * or if you do not configure it to manage its own timer/task, then you can call - * update() within your own function to update the state of the encoder. + * timer (a HighResolutionTimer by default, changeable to an espp::Timer via + * KConfig / menuconfig), or if you do not configure it to manage its own timer, + * then you can call update() within your own function to update the state of + * the encoder. * * @warning You should not call update() if you have configured the encoder to - * use its own timer/task or if you have called start() yourself. + * use its own timer or if you have called start() yourself. + * + * @note The AS5600 has 12-bit angular resolution (4096 counts / revolution). * * @note There is an implicit assumption in this class regarding the maximum * velocity it can measure (above which there will be aliasing). The - * fastest velocity it can measure will be (0.5f * update_period * 60.0f) + * fastest velocity it can measure will be (0.5f / update_period * 60.0f) * which is half a rotation in one update period. * * @note The assumption above also affects the reliability of the accumulator, @@ -39,355 +47,74 @@ namespace espp { * \section as5600_ex1 As5600 Example * \snippet as5600_example.cpp as5600 example */ -class As5600 : public BasePeripheral<> { +class As5600 : public MagneticEncoderBase { public: static constexpr uint8_t DEFAULT_ADDRESS = (0b0110110); ///< I2C address of the AS5600 - /** - * @brief Filter the input raw velocity and return it. - * @param raw Most recent raw velocity measured. - * @return Filtered velocity. - */ - typedef std::function velocity_filter_fn; - - static constexpr int COUNTS_PER_REVOLUTION = - 16384; ///< Int number of counts per revolution for the magnetic encoder. - static constexpr float COUNTS_PER_REVOLUTION_F = - 16384.0f; ///< Float number of counts per revolution for the magnetic encoder. - static constexpr float COUNTS_TO_RADIANS = - 2.0f * (float)(M_PI) / - COUNTS_PER_REVOLUTION_F; ///< Conversion factor to convert from count value to radians. - static constexpr float COUNTS_TO_DEGREES = - 360.0f / - COUNTS_PER_REVOLUTION_F; ///< Conversion factor to convert from count value to degrees. - static constexpr float SECONDS_PER_MINUTE = - 60.0f; ///< Conversion factor to convert from seconds to minutes. - - static constexpr int MIN_DIFF = - CONFIG_AS5600_MIN_DIFF; ///< Minimum difference for velocity calculation. + /// @brief The CRTP base type providing the shared encoder machinery. + using Base = + MagneticEncoderBase; /** * @brief Configuration information for the As5600. */ struct Config { uint8_t device_address = DEFAULT_ADDRESS; ///< I2C address for this device. - BasePeripheral::write_then_read_fn + BasePeripheral<>::write_then_read_fn write_then_read; ///< Function to write then read from the device. velocity_filter_fn velocity_filter{nullptr}; ///< Function to filter the veolcity. @note Will be ///< called once every update_period seconds. std::chrono::duration update_period{ .01f}; ///< Update period (1/sample rate) in seconds. This determines the periodicity of the - ///< task which will read the position, update the accumulator, and update/filter + ///< timer which will read the position, update the accumulator, and update/filter ///< velocity. bool auto_init{true}; ///< Whether to automatically initialize the accumulator to the current ///< position on startup. - bool run_task{true}; ///< Whether to run the task on startup. If false, you must call update() + bool run_task{true}; ///< Whether to run the timer on startup. If false, you must call update() ///< manually. Logger::Verbosity log_level{Logger::Verbosity::WARN}; }; /** - * @brief Construct the As5600 and start the update task if auto_init and run_task are true. + * @brief Construct the As5600 and start the update timer if auto_init and run_task are true. * @param config Configuration for the As5600. */ explicit As5600(const Config &config) - : BasePeripheral( - {.address = config.device_address, .write_then_read = config.write_then_read}, "As5600", - config.log_level) - , velocity_filter_(config.velocity_filter) - , update_period_(config.update_period) { + : Base("As5600", config.velocity_filter, config.update_period, config.log_level) { + set_address(config.device_address); + set_write_then_read(config.write_then_read); if (config.auto_init) { std::error_code ec; initialize(config.run_task, ec); } } -#if !defined(CONFIG_AS5600_USE_TIMER) || defined(_DOXYGEN_) - /** - * @brief Construct the As5600 and start the update task/timer if auto_init and run_task are true. - * @param config Configuration for the As5600. - * @param task_config Configuration for the internal task. - */ - explicit As5600(const Config &config, const espp::Task::Config &task_config) - : BasePeripheral( - {.address = config.device_address, .write_then_read = config.write_then_read}, "As5600", - config.log_level) - , velocity_filter_(config.velocity_filter) - , update_period_(config.update_period) - , task_(espp::Task::make_unique(task_config)) { - if (config.auto_init) { - std::error_code ec; - initialize(config.run_task, ec); - } - } -#endif - - /** - * @brief Initialize the accumulator to the current position and start the - * update task. - * @param ec Error code to set if there is an error. - * @note This version of initialize() starts the update task, so you do not - * need to call update() manually. - */ - void initialize(std::error_code &ec) { initialize(true, ec); } - - /** - * @brief Initialize the accumulator to the current position and start the - * update task, if desired. - * @param run_task Whether to start the update task. - * @param ec Error code to set if there is an error. - * @note If you do not start the task, you must call update() manually. - */ - void initialize(bool run_task, std::error_code &ec) { - logger_.info("Initializing. Fastest measurable velocity will be {:.3f} RPM", - // half a rotation in one update period is the fastest we can - // measure - 0.5f / update_period_.count() * SECONDS_PER_MINUTE); - init(run_task, ec); - if (ec) { - logger_.error("Error initializing: {}", ec.message()); - } - } - -#if !defined(CONFIG_AS5600_USE_TIMER) || defined(_DOXYGEN_) - /** - * @brief Initialize the accumulator to the current position and start the - * update task, if desired. - * @param run_task Whether to start the update task. - * @param task_config Configuration for the internal task. - * @param ec Error code to set if there is an error. - * @note If you do not start the task, you must call update() manually. - */ - void initialize(bool run_task, const espp::Task::Config &task_config, std::error_code &ec) { - // create the task (discard any previous one) - task_.reset(); - task_ = espp::Task::make_unique(task_config); - initialize(run_task, ec); - } -#endif - - /** - * @brief Return whether the sensor needs to search for absolute 0 on startup. - * @note The AS5600 (using I2C/SPI) does not need to search for absolute 0 - * and will always know it on startup. Therefore this function always - * returns false. - * @return False because the magnetic sensor (using I2C/SPI) does not need to - * search for 0. - */ - bool needs_zero_search() const { return false; } - - /** - * @brief Get the most recently updated raw count value from the encoder. - * @note This value always represents the angle of the encoder modulo one - * rotation, meaning it only represents the range 0 to 360 degrees. It - * is not recommended to use this function, but is provided for edge use - * cases. - * @return Raw count value in the range [0, 16384] (0 to 360 degrees). - */ - int get_count() const { return count_.load(); } - - /** - * @brief Return the accumulated count that the encoder has generated since it - * was initialized. - * @note This value is a raw counter value that can be +/-, meaning - * COUNTS_PER_REVOLUTION can be used to convert it to revolutions. - * @return Raw accumulator value. - */ - int get_accumulator() const { return accumulator_.load(); } - - /** - * @brief Reset the accumulator to zero. - */ - void reset_accumulator() { accumulator_ = 0; } - - /** - * @brief Return the mechanical / shaft angle of the encoder, in radians, - * within the range [0, 2pi]. - * @return Angle in radians of the encoder within the range [0, 2pi]. - */ - float get_mechanical_radians() const { return (float)get_count() * COUNTS_TO_RADIANS; } - - /** - * @brief Return the mechanical / shaft angle of the encoder, in degrees, - * within the range [0, 360]. - * @return Angle in degrees of the encoder within the range [0, 360]. - */ - float get_mechanical_degrees() const { return (float)get_count() * COUNTS_TO_DEGREES; } - - /** - * @brief Return the accumulated position of the encoder, in radians. - * @note This can be any value, it is not restricted to [-2pi, 2pi]. - * @return Position in radians of the encoder. - */ - float get_radians() const { return (float)get_accumulator() * COUNTS_TO_RADIANS; } - - /** - * @brief Return the accumulated position of the encoder, in degrees. - * @note This can be any value, it is not restricted to [-360, 360]. - * @return Position in degrees of the encoder. - */ - float get_degrees() const { return (float)get_accumulator() * COUNTS_TO_DEGREES; } - - /** - * @brief Return the filtered velocity of the encoder, in RPM. - * @return Filtered velocity (revolutions / minute, RPM). - */ - float get_rpm() const { return velocity_rpm_.load(); } - - /** - * @brief Update the state of the encoder by reading the latest data from the - * encoder and updating the associated state. - * @param ec Error code to set if there is an error. - * @note You should not call this function if you have started the encoder's - * update task (e.g. run_task = true in the constructor, or you called - * initialize(true)). - */ - void update(std::error_code &ec) { - std::lock_guard lock(base_mutex_); - // measure update timing - uint64_t now_us = esp_timer_get_time(); - auto dt = now_us - prev_time_us_; - float seconds = dt / 1e6f; - prev_time_us_ = now_us; - // store the previous count - int prev_count = count_; - // update raw count - auto count = read_count(ec); - if (ec) { - return; - } - count_.store(count); - // compute diff - int diff = count_ - prev_count; - // check for zero crossing - if (diff > COUNTS_PER_REVOLUTION / 2) { - // we crossed zero going clockwise (1 -> 359) - diff -= COUNTS_PER_REVOLUTION; - } else if (diff < -COUNTS_PER_REVOLUTION / 2) { - // we crossed zero going counter-clockwise (359 -> 1) - diff += COUNTS_PER_REVOLUTION; - } - // update accumulator - accumulator_ += diff; - logger_.debug_rate_limited("CDA: {}, {}, {}", count_, diff, accumulator_); - // update velocity (filtering it) - float raw_velocity = - (dt > 0 && std::abs(diff) > MIN_DIFF) - ? (float)(diff) / COUNTS_PER_REVOLUTION_F / seconds * SECONDS_PER_MINUTE - : 0.0f; - velocity_rpm_ = velocity_filter_ ? velocity_filter_(raw_velocity) : raw_velocity; - if (dt > 0) { - float max_velocity = 0.5f / seconds * SECONDS_PER_MINUTE; - if (raw_velocity >= max_velocity) { - logger_.warn_rate_limited( - "Velocity nearing measurement limit ({:.3f} RPM), consider decreasing your " - "update period!", - max_velocity); - } - } - } - - /** - * @brief Start the update task/timer. - * @note This will start the task/timer that calls update() at the update_period. - * @note This is only useful if you previously stopped the task/timer or if you - * initialized with run_task = false. - * @return True if the task/timer was started successfully, false otherwise. - */ - bool start() { - logger_.info("Starting task with update period of {:.3f} seconds", update_period_.count()); - prev_time_us_ = esp_timer_get_time(); -#if defined(CONFIG_AS5600_USE_TIMER) - uint64_t period_us = - std::chrono::duration_cast(update_period_).count(); - return timer_.periodic(period_us); -#else - if (!task_) { - return false; - } - return task_->start(); -#endif - } - - /** - * @brief Stop the update task/timer. - * @note This will stop the task/timer that calls update() at the update_period. - * @note After stopping, you can manually call update() or restart with start(). - */ - void stop() { - logger_.info("Stopping task"); -#if defined(CONFIG_AS5600_USE_TIMER) - timer_.stop(); -#else - if (task_) { - task_->stop(); - } -#endif - } + /// @brief Stop the update timer before the object is destroyed. + ~As5600() { stop(); } protected: - int read_count(std::error_code &ec) { + // Allow the base to invoke our (protected) read() via CRTP static dispatch. + friend Base; + + /// @brief Read the current 12-bit angle and update count_. + /// @param ec Error code to set if there is an error. + void read(std::error_code &ec) { std::lock_guard lock(base_mutex_); // read the angle count registers uint8_t angle_h = read_u8_from_register((uint8_t)Registers::ANGLE_H, ec); if (ec) { logger_.error_rate_limited("Error reading: {}", ec.message()); - return 0; + return; } - uint8_t angle_l = read_u8_from_register((uint8_t)Registers::ANGLE_L, ec) >> 2; + uint8_t angle_l = read_u8_from_register((uint8_t)Registers::ANGLE_L, ec); if (ec) { logger_.error_rate_limited("Error reading: {}", ec.message()); - return 0; - } - return (int)((angle_h << 6) | angle_l); - } - -#if defined(CONFIG_AS5600_USE_TIMER) - bool update_task() { - std::error_code ec; - update(ec); - if (ec) { - logger_.error("Error updating: {}", ec.message()); - } - // don't want to stop the task - return false; - } -#else - bool update_task(std::mutex &m, std::condition_variable &cv, bool &task_notified) { - auto start_time = std::chrono::high_resolution_clock::now(); - std::error_code ec; - update(ec); - if (ec) { - logger_.error("Error updating: {}", ec.message()); - } - // sleep until the next update period - { - std::unique_lock lk(m); - cv.wait_until(lk, start_time + update_period_, [&task_notified] { return task_notified; }); - task_notified = false; - } - // don't want to stop the task - return false; - } -#endif - - void init(bool run_task, std::error_code &ec) { - std::lock_guard lock(base_mutex_); - // initialize the accumulator to have the current angle - auto count = read_count(ec); - if (ec) { return; } - accumulator_ = count; - if (!run_task) { - logger_.info( - "Not starting task, run_task is false. Manually call update() to update the state."); - return; - } - if (!start()) { - logger_.error("Error starting task"); - ec = make_error_code(std::errc::operation_not_permitted); - } + // The AS5600 ANGLE is a 12-bit value: ANGLE_H holds Angle[11:8] in its low + // nibble, ANGLE_L holds Angle[7:0]. + count_ = ((angle_h & 0x0F) << 8) | angle_l; } /** @@ -433,22 +160,5 @@ class As5600 : public BasePeripheral<> { static constexpr int MAGNET_HIGH = (1 << 3); ///< For use with the STATUS register static constexpr int MAGNET_LOW = (1 << 4); ///< For use with the STATUS register static constexpr int MAGNET_DETECTED = (1 << 5); ///< For use with the STATUS register - - velocity_filter_fn velocity_filter_{nullptr}; - uint64_t prev_time_us_{0}; - std::chrono::duration update_period_; - std::atomic count_{0}; - std::atomic accumulator_{0}; - std::atomic velocity_rpm_{0}; -#if defined(CONFIG_AS5600_USE_TIMER) - espp::HighResolutionTimer timer_{ - {.name = "As5600", - .callback = std::bind(&As5600::update_task, this) }}; -#else - std::unique_ptr task_ = espp::Task::make_unique(Task::Config{ - .callback = std::bind_front(&As5600::update_task, this), - .task_config = {.name = "As5600"}, - }); -#endif }; } // namespace espp diff --git a/components/esp32-p4-eth/idf_component.yml b/components/esp32-p4-eth/idf_component.yml index e3edb9c0c7..fa58a765ae 100644 --- a/components/esp32-p4-eth/idf_component.yml +++ b/components/esp32-p4-eth/idf_component.yml @@ -33,7 +33,8 @@ dependencies: espp/interrupt: ">=1.0" # MIPI-CSI camera pipeline: esp_video provides the V4L2 capture framework # (CSI + ISP) and esp_cam_sensor provides the OV5647 sensor driver. - espressif/esp_video: ">=2.0" - espressif/esp_cam_sensor: ">=2.0" + espressif/esp_video: ">=2.0,<2.4" # 2.4.0 fails to compile against the CI IDF (ISP_LL_EVENT_ERROR_MASK undeclared) + espressif/esp_ipa: ">=2.0,<2.3" # prebuilt 2.3.0 illegal-instructions at runtime on P4 + espressif/esp_cam_sensor: ">=2.0,<2.4" targets: - esp32p4 diff --git a/components/esp32-p4-nano/idf_component.yml b/components/esp32-p4-nano/idf_component.yml index 991b4ba6d9..e288661b16 100644 --- a/components/esp32-p4-nano/idf_component.yml +++ b/components/esp32-p4-nano/idf_component.yml @@ -33,7 +33,8 @@ dependencies: espp/interrupt: ">=1.0" # MIPI-CSI camera pipeline: esp_video provides the V4L2 capture framework # (CSI + ISP) and esp_cam_sensor provides the OV5647 sensor driver. - espressif/esp_video: ">=2.0" - espressif/esp_cam_sensor: ">=2.0" + espressif/esp_video: ">=2.0,<2.4" # 2.4.0 fails to compile against the CI IDF (ISP_LL_EVENT_ERROR_MASK undeclared) + espressif/esp_ipa: ">=2.0,<2.3" # prebuilt 2.3.0 illegal-instructions at runtime on P4 + espressif/esp_cam_sensor: ">=2.0,<2.4" targets: - esp32p4 diff --git a/components/magnetic_encoder/CMakeLists.txt b/components/magnetic_encoder/CMakeLists.txt new file mode 100644 index 0000000000..13393208a9 --- /dev/null +++ b/components/magnetic_encoder/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register( + INCLUDE_DIRS "include" + REQUIRES "base_peripheral" "timer" + ) diff --git a/components/magnetic_encoder/README.md b/components/magnetic_encoder/README.md new file mode 100644 index 0000000000..49c84df594 --- /dev/null +++ b/components/magnetic_encoder/README.md @@ -0,0 +1,28 @@ +# Magnetic Encoder Base + +[![Badge](https://components.espressif.com/components/espp/magnetic_encoder/badge.svg)](https://components.espressif.com/components/espp/magnetic_encoder) + +The `espp::MagneticEncoderBase` class is the shared CRTP base class for the espp +magnetic angle encoders (`espp::As5600` and `espp::Mt6701`). It holds all of the +machinery common to those encoders: + +* the periodic update loop (raw-count accumulation + velocity estimation), +* the position / velocity accessors (count, radians, degrees, accumulator, RPM), +* and the periodic driver that calls `update()` at the configured rate. + +The concrete encoder supplies exactly one thing - a `read()` that refreshes the +raw count from the sensor - which the base invokes through static (CRTP) +dispatch, so there is no virtual-call overhead even when the update loop runs at +very high frequency (e.g. 1-2 kHz). + +The periodic driver is selected at compile time (per encoder, via KConfig / +menuconfig): + +* an `espp::HighResolutionTimer` (default), which has microsecond resolution and + is the right choice for sub-millisecond update periods, or +* an `espp::Timer`, which schedules against an absolute wake-up time (drift-free + and in phase - important for stable velocity / accumulator state) but is + limited to the FreeRTOS tick resolution. + +This component is a building block; it is not instantiated directly. See the +`as5600` and `mt6701` components for concrete encoders that derive from it. diff --git a/components/magnetic_encoder/idf_component.yml b/components/magnetic_encoder/idf_component.yml new file mode 100644 index 0000000000..968607c58c --- /dev/null +++ b/components/magnetic_encoder/idf_component.yml @@ -0,0 +1,19 @@ +## IDF Component Manager Manifest File +license: "MIT" +description: "Shared CRTP base class for espp magnetic angle encoders (As5600, Mt6701): periodic position/velocity update loop over a HighResolutionTimer or Timer" +url: "https://github.com/esp-cpp/espp/tree/main/components/magnetic_encoder" +repository: "git://github.com/esp-cpp/espp.git" +maintainers: + - William Emfinger +documentation: "https://esp-cpp.github.io/espp/encoder/index.html" +tags: + - cpp + - Component + - Encoder + - Magnetic + - Peripheral +dependencies: + idf: + version: '>=5.0' + espp/base_peripheral: '>=1.0' + espp/timer: '>=1.0' diff --git a/components/magnetic_encoder/include/magnetic_encoder_base.hpp b/components/magnetic_encoder/include/magnetic_encoder_base.hpp new file mode 100644 index 0000000000..42ee2aa0b4 --- /dev/null +++ b/components/magnetic_encoder/include/magnetic_encoder_base.hpp @@ -0,0 +1,363 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "base_peripheral.hpp" +#include "high_resolution_timer.hpp" +#include "timer.hpp" + +namespace espp { +/** + * @brief CRTP base class for magnetic angle encoders (e.g. As5600, Mt6701). + * + * This class holds all of the machinery shared by the magnetic encoders: the + * periodic update loop (raw-count accumulation + velocity estimation), the + * position / velocity accessors, and the periodic driver that calls update() at + * the configured rate. The concrete encoder supplies exactly one thing - a + * `read(std::error_code&)` that refreshes `count_` from the sensor (and any + * device-specific state) - which the base invokes through static (CRTP) + * dispatch, so there is no virtual-call overhead even when the update loop runs + * at very high frequency (e.g. 1-2 kHz). + * + * The periodic driver is selected at compile time via @p UseHighResTimer: + * - `true` (default in the encoders' Kconfig): an esp_timer-backed + * espp::HighResolutionTimer, which has microsecond resolution and is the + * right choice for sub-millisecond update periods. + * - `false`: an espp::Timer, which schedules against an absolute wake-up time + * (the k-th callback targets `start + k * period`) so it is periodic and in + * phase - critical for stable velocity / accumulator state - but is limited + * to the FreeRTOS tick resolution. + * + * @warning You should not call update() yourself if you have configured the + * encoder to run its own timer (run_task = true) or if you have called + * start(). + * + * @note There is an implicit assumption regarding the maximum velocity that can + * be measured (above which there will be aliasing). The fastest velocity + * that can be measured is `0.5 / update_period * 60` RPM, i.e. half a + * rotation in one update period. This also bounds the reliability of the + * accumulator, since it accumulates position differences every update. + * + * @tparam Derived The concrete encoder class (CRTP). Must provide + * `void read(std::error_code&)` which updates `count_`. + * @tparam UseHighResTimer If true, drive updates with HighResolutionTimer; + * if false, drive with espp::Timer. + * @tparam CountsPerRevolution Number of raw counts per mechanical revolution + * (e.g. 4096 for the 12-bit As5600, 16384 for the 14-bit Mt6701). + * @tparam MinDiff Minimum count difference required to update the velocity + * estimate; smaller differences are treated as zero velocity to reject + * noise / jitter. + * @tparam RegisterAddressType Register address type for BasePeripheral. + * @tparam UseAddress Whether the peripheral is addressed (I2C) or not (SSI). + */ +template +class MagneticEncoderBase : public BasePeripheral { +public: + /// @brief The periodic driver type selected by @p UseHighResTimer. + using TimerType = std::conditional_t; + + /** + * @brief Filter the input raw velocity and return it. + * @param raw Most recent raw velocity measured. + * @return Filtered velocity. + */ + typedef std::function velocity_filter_fn; + + static constexpr int COUNTS_PER_REVOLUTION = + CountsPerRevolution; ///< Int number of counts per revolution for the magnetic encoder. + static constexpr float COUNTS_PER_REVOLUTION_F = + (float)CountsPerRevolution; ///< Float number of counts per revolution. + static constexpr float COUNTS_TO_RADIANS = + 2.0f * (float)(M_PI) / + COUNTS_PER_REVOLUTION_F; ///< Conversion factor to convert from count value to radians. + static constexpr float COUNTS_TO_DEGREES = + 360.0f / + COUNTS_PER_REVOLUTION_F; ///< Conversion factor to convert from count value to degrees. + static constexpr float SECONDS_PER_MINUTE = + 60.0f; ///< Conversion factor to convert from seconds to minutes. + static constexpr int MIN_DIFF = MinDiff; ///< Minimum difference for velocity calculation. + + /** + * @brief Return whether the sensor needs to search for absolute 0 on startup. + * @note Magnetic angle encoders (using I2C / SPI) always know their absolute + * angle on startup, so this always returns false. + * @return False. + */ + bool needs_zero_search() const { return false; } + + /** + * @brief Get the most recently updated raw count value from the encoder. + * @note This value always represents the angle of the encoder modulo one + * rotation, meaning it only represents the range 0 to 360 degrees. + * @return Raw count value in the range [0, COUNTS_PER_REVOLUTION). + */ + int get_count() const { return count_.load(); } + + /** + * @brief Return the accumulated count generated since initialization. + * @note This value is a raw counter value that can be +/-; divide by + * COUNTS_PER_REVOLUTION to convert it to revolutions. It is stored as a + * 64-bit value so it does not overflow during long, continuous rotation. + * @return Raw accumulator value. + */ + int64_t get_accumulator() const { return accumulator_.load(); } + + /** + * @brief Reset the accumulator to zero. + */ + void reset_accumulator() { accumulator_ = 0; } + + /** + * @brief Return the mechanical / shaft angle of the encoder, in radians, + * within the range [0, 2pi]. + * @return Angle in radians of the encoder within the range [0, 2pi]. + */ + float get_mechanical_radians() const { return (float)get_count() * COUNTS_TO_RADIANS; } + + /** + * @brief Return the mechanical / shaft angle of the encoder, in degrees, + * within the range [0, 360]. + * @return Angle in degrees of the encoder within the range [0, 360]. + */ + float get_mechanical_degrees() const { return (float)get_count() * COUNTS_TO_DEGREES; } + + /** + * @brief Return the accumulated position of the encoder, in radians. + * @note This can be any value, it is not restricted to [-2pi, 2pi]. + * @return Position in radians of the encoder. + */ + float get_radians() const { return (float)get_accumulator() * COUNTS_TO_RADIANS; } + + /** + * @brief Return the accumulated position of the encoder, in degrees. + * @note This can be any value, it is not restricted to [-360, 360]. + * @return Position in degrees of the encoder. + */ + float get_degrees() const { return (float)get_accumulator() * COUNTS_TO_DEGREES; } + + /** + * @brief Return the filtered velocity of the encoder, in RPM. + * @return Filtered velocity (revolutions / minute, RPM). + */ + float get_rpm() const { return velocity_rpm_.load(); } + + /** + * @brief Initialize the accumulator to the current position and start the + * update timer. + * @param ec Error code to set if there is an error. + */ + void initialize(std::error_code &ec) { initialize(true, ec); } + + /** + * @brief Initialize the accumulator to the current position and start the + * update timer, if desired. + * @param run_task Whether to start the update timer. + * @param ec Error code to set if there is an error. + * @note If you do not start the timer, you must call update() manually. + */ + void initialize(bool run_task, std::error_code &ec) { + logger_.info("Initializing. Fastest measurable velocity will be {:.3f} RPM", + // half a rotation in one update period is the fastest we can measure + 0.5f / update_period_.count() * SECONDS_PER_MINUTE); + init(run_task, ec); + if (ec) { + logger_.error("Error initializing: {}", ec.message()); + } + } + + /** + * @brief Update the state of the encoder by reading the latest data from the + * encoder and updating the associated state. + * @param ec Error code to set if there is an error. + * @note You should not call this function if you have started the encoder's + * update timer (e.g. run_task = true, or you called start()). + */ + void update(std::error_code &ec) { + std::lock_guard lock(base_mutex_); + // sample the timestamp and previous count before the read; both are only + // committed once the read succeeds + uint64_t now_us = esp_timer_get_time(); + int prev_count = count_; + // refresh count_ (and any device-specific state) via CRTP static dispatch + static_cast(this)->read(ec); + if (ec) { + // leave prev_time_us_ untouched so the next successful sample measures its + // movement over the time since the last good read, not since this failed + // attempt (which would inflate the velocity / aliasing check) + return; + } + // measure update timing + auto dt = now_us - prev_time_us_; + float seconds = dt / 1e6f; + prev_time_us_ = now_us; + // compute diff + int diff = count_ - prev_count; + // check for zero crossing + if (diff > COUNTS_PER_REVOLUTION / 2) { + // we crossed zero going clockwise (1 -> 359) + diff -= COUNTS_PER_REVOLUTION; + } else if (diff < -COUNTS_PER_REVOLUTION / 2) { + // we crossed zero going counter-clockwise (359 -> 1) + diff += COUNTS_PER_REVOLUTION; + } + // update accumulator + accumulator_ += diff; + logger_.debug_rate_limited("CDA: {}, {}, {}", count_.load(), diff, accumulator_.load()); + // update velocity (filtering it) + float raw_velocity = + (dt > 0 && std::abs(diff) > MIN_DIFF) + ? (float)(diff) / COUNTS_PER_REVOLUTION_F / seconds * SECONDS_PER_MINUTE + : 0.0f; + velocity_rpm_ = velocity_filter_ ? velocity_filter_(raw_velocity) : raw_velocity; + if (dt > 0) { + float max_velocity = 0.5f / seconds * SECONDS_PER_MINUTE; + // compare magnitude so the limit is caught for both rotation directions + if (std::abs(raw_velocity) >= max_velocity) { + logger_.warn_rate_limited( + "Velocity nearing measurement limit ({:.3f} RPM), consider decreasing your " + "update period!", + max_velocity); + } + } + } + + /** + * @brief Start the update timer. + * @note This will start the timer that calls update() at the update_period. + * @note This is only useful if you previously stopped the timer or if you + * initialized with run_task = false. + * @return True if the timer was started successfully, false otherwise. + */ + bool start() { + logger_.info("Starting update timer with period of {:.3f} seconds", update_period_.count()); + prev_time_us_ = esp_timer_get_time(); + if (!timer_) { + return false; + } + if constexpr (UseHighResTimer) { + uint64_t period_us = + std::chrono::duration_cast(update_period_).count(); + return timer_->periodic(period_us); + } else { + timer_->set_period(update_period_); + return timer_->start(); + } + } + + /** + * @brief Stop the update timer. + * @note This will stop the timer that calls update() at the update_period. + * @note After stopping, you can manually call update() or restart with start(). + */ + void stop() { + logger_.info("Stopping update timer"); + if (timer_) { + timer_->stop(); + } + } + +protected: + using Base = BasePeripheral; + using Base::base_mutex_; + using Base::logger_; + + /** + * @brief Construct the base, forwarding an empty peripheral config. + * @param name Name used for the peripheral and its update timer. + * @param velocity_filter Optional velocity filter, called once per update. + * @param update_period Update period (1 / sample rate) in seconds. + * @param log_level Log verbosity. + * @note The concrete encoder must set the transport (write / read / + * write_then_read / address) in its own constructor body, then call + * initialize(). The update timer is created here but never auto-starts; + * start() (via initialize()) is authoritative. + */ + MagneticEncoderBase(std::string_view name, const velocity_filter_fn &velocity_filter, + const std::chrono::duration &update_period, + espp::Logger::Verbosity log_level) + : Base({}, name, log_level) + , velocity_filter_(velocity_filter) + , update_period_(update_period) { + // Construct the timer here in the constructor body (after update_period_ is + // set) so the espp::Timer branch can read the period. auto_start is false so + // that the timer does not fire until start() is called. + if constexpr (UseHighResTimer) { + // Use the same bool-returning lambda as the espp::Timer branch below: a + // bool-returning callable converts to std::function (the current + // HighResolutionTimer callback type, which discards the result) AND to + // std::function, so this stays valid even if the HRT callback + // signature is ever aligned with espp::Timer's. + timer_ = std::make_unique(espp::HighResolutionTimer::Config{ + .name = std::string(name), .callback = [this]() { return update_task(); }}); + } else { + timer_ = std::make_unique( + espp::Timer::Config{.name = name, + .period = update_period_, + .callback = [this]() { return update_task(); }, + .auto_start = false}); + } + } + + /** + * @brief The periodic callback: run one update(). + * @return Always false (never cancels the timer). + * @note For the HighResolutionTimer branch the bool return is ignored; for the + * espp::Timer branch, returning false keeps the timer running. + */ + bool update_task() { + std::error_code ec; + update(ec); + if (ec) { + logger_.error("Error updating: {}", ec.message()); + } + // don't want to stop the timer + return false; + } + + /** + * @brief Seed the accumulator from the current angle and (optionally) start. + * @param run_task Whether to start the update timer. + * @param ec Error code to set if there is an error. + */ + void init(bool run_task, std::error_code &ec) { + std::lock_guard lock(base_mutex_); + // initialize the accumulator to have the current angle + static_cast(this)->read(ec); + if (ec) { + return; + } + accumulator_ = count_.load(); + // seed the timestamp so the first update() (in particular a manual update() + // when run_task is false) measures dt from now, not from boot. start() also + // refreshes it for timer mode. + prev_time_us_ = esp_timer_get_time(); + if (!run_task) { + logger_.info( + "Not starting timer, run_task is false. Manually call update() to update the state."); + return; + } + if (!start()) { + logger_.error("Error starting update timer"); + ec = make_error_code(std::errc::operation_not_permitted); + } + } + + velocity_filter_fn velocity_filter_{nullptr}; + uint64_t prev_time_us_{0}; + std::chrono::duration update_period_; + std::atomic count_{0}; + std::atomic accumulator_{0}; + std::atomic velocity_rpm_{0}; + std::unique_ptr timer_; +}; +} // namespace espp diff --git a/components/mt6701/CMakeLists.txt b/components/mt6701/CMakeLists.txt index 325405646b..e1ec22a324 100644 --- a/components/mt6701/CMakeLists.txt +++ b/components/mt6701/CMakeLists.txt @@ -1,4 +1,4 @@ idf_component_register( INCLUDE_DIRS "include" - REQUIRES "base_peripheral" "timer" "task" + REQUIRES "magnetic_encoder" "base_peripheral" "timer" ) diff --git a/components/mt6701/Kconfig b/components/mt6701/Kconfig index 6c46ca2f4b..082e209a30 100644 --- a/components/mt6701/Kconfig +++ b/components/mt6701/Kconfig @@ -10,12 +10,15 @@ menu "MT6701 Configuration" the velocity will be set to 0. This helps filter out noise and small jitter in the encoder readings. - config MT6701_USE_TIMER - bool "Use high resolution timer instead of task" + config MT6701_USE_HIGH_RESOLUTION_TIMER + bool "Use high resolution timer instead of software timer" default y help - Use the high resolution timer instead of a FreeRTOS task for - periodic updates. The timer is more precise and has lower overhead. - Disable this if you prefer to use a task-based implementation. + Drive the periodic encoder update with an esp_timer-backed + HighResolutionTimer (microsecond resolution). This is the right + choice for sub-millisecond update periods. Disable this to instead + use an espp::Timer, which schedules against an absolute wake-up time + (drift-free and in phase) but is limited to the FreeRTOS tick + resolution. endmenu diff --git a/components/mt6701/README.md b/components/mt6701/README.md index 0875ce7c21..ca921680ff 100644 --- a/components/mt6701/README.md +++ b/components/mt6701/README.md @@ -13,9 +13,11 @@ measure * Accumulated degrees (since the component was created) * Speed (rotations per minute / RPM) -It does so by spawning a task which periodically reads the magnetic encoder, +It does so by spawning a timer which periodically reads the magnetic encoder, updates the accumulator, and computes the velocity. The component can be -configured to optionally filter the velocity. +configured to optionally filter the velocity. The timer can be either a +`espp::HighResolutionTimer` (default) or an `espp::Timer`, selected via KConfig / +menuconfig. The periodicity / update rate of the encoder can be configured at time of creation. diff --git a/components/mt6701/example/README.md b/components/mt6701/example/README.md index e84f4da6ad..d823e743ee 100644 --- a/components/mt6701/example/README.md +++ b/components/mt6701/example/README.md @@ -5,8 +5,9 @@ MT6701 magnetic encoder chip. ## How to use example -It uses the `task` component to periodically read the raw count, position, and -velocity of the encoder, the `filters` component (specifically the +It uses the `task` component to periodically poll and print the raw count, +position, and velocity of the encoder (the `Mt6701` itself maintains that state +with its own internal timer), the `filters` component (specifically the `espp::ButterworthFilter` class) to filter the raw values from the sensor, and the `format` component to print the data to the console in CSV format. diff --git a/components/mt6701/idf_component.yml b/components/mt6701/idf_component.yml index ae87d801f0..3c8e85b1ad 100644 --- a/components/mt6701/idf_component.yml +++ b/components/mt6701/idf_component.yml @@ -20,5 +20,6 @@ tags: dependencies: idf: version: '>=5.0' + espp/magnetic_encoder: '>=1.0' espp/base_peripheral: '>=1.0' espp/timer: '>=1.0' diff --git a/components/mt6701/include/mt6701.hpp b/components/mt6701/include/mt6701.hpp index ac5a733a1b..58cc2991dd 100644 --- a/components/mt6701/include/mt6701.hpp +++ b/components/mt6701/include/mt6701.hpp @@ -1,14 +1,11 @@ #pragma once #include -#include -#include +#include #include -#include "base_peripheral.hpp" -#include "high_resolution_timer.hpp" -#include "task.hpp" +#include "magnetic_encoder_base.hpp" namespace espp { /// @brief Enum class for the interface type of the MT6701. @@ -16,6 +13,15 @@ enum class Mt6701Interface : uint8_t { I2C = 0, ///< Inter-Integrated Circuit (I2C) SSI = 1, ///< Synchronous Serial Interface (SSI), which can be SPI or SSI }; + +/// @brief Whether the Mt6701 drives its update loop with a HighResolutionTimer +/// (true) or an espp::Timer (false). Selected by Kconfig / menuconfig. +#if defined(CONFIG_MT6701_USE_HIGH_RESOLUTION_TIMER) +inline constexpr bool mt6701_use_high_resolution_timer = true; +#else +inline constexpr bool mt6701_use_high_resolution_timer = false; +#endif + /** * @brief Class for position and velocity measurement using a MT6701 magnetic * encoder. This class starts its own measurement task at the specified @@ -24,15 +30,18 @@ enum class Mt6701Interface : uint8_t { * SSI, ABZ, UVW, Analog/PWM, and Push-Button interfaces. * * This component can be configured to automatically update within its own - * timer/task (timer is default, and can be changed via KConfig / menuconfig), - * or if you do not configure it to manage its own timer/task, then you can call - * update() within your own function to update the state of the encoder. + * timer (a HighResolutionTimer by default, changeable to an espp::Timer via + * KConfig / menuconfig), or if you do not configure it to manage its own timer, + * then you can call update() within your own function to update the state of + * the encoder. * * @warning You should not call update() if you have configured the encoder to - * use its own timer/task or if you have called start() yourself. + * use its own timer or if you have called start() yourself. * * @note This implementation currently only supports I2C and SSI interfaces. * + * @note The MT6701 has 14-bit angular resolution (16384 counts / revolution). + * * @note There is an implicit assumption in this class regarding the maximum * velocity it can measure (above which there will be aliasing). The * fastest velocity it can measure will be (0.5f / update_period * 60.0f) @@ -48,32 +57,34 @@ enum class Mt6701Interface : uint8_t { * \snippet mt6701_example.cpp mt6701 ssi example */ template -class Mt6701 : public BasePeripheral { - // Since the BasePeripheral is a dependent base class (e.g. its template - // parameters depend on our template parameters), we need to use the `using` - // keyword to bring in the functions / members we want to use, otherwise we - // have to either use `this->` or explicitly scope each call, which clutters - // the code / is annoying. This is needed because of the two phases of name - // lookups for templates. - using BasePeripheral::set_address; - using BasePeripheral::set_write; - using BasePeripheral::set_read; - using BasePeripheral::read_u8_from_register; - using BasePeripheral::read; - using BasePeripheral::logger_; - using BasePeripheral::base_mutex_; +class Mt6701 : public MagneticEncoderBase, mt6701_use_high_resolution_timer, + 16384, CONFIG_MT6701_MIN_DIFF, uint8_t, + Interface == Mt6701Interface::I2C> { + // Since the base class is a dependent base (its template parameters depend on + // ours), we bring in the base / grand-base members we use with `using` + // declarations, otherwise we would have to scope each call with `this->`. This + // is needed because of the two-phase name lookup for templates. + using Base = + MagneticEncoderBase, mt6701_use_high_resolution_timer, 16384, + CONFIG_MT6701_MIN_DIFF, uint8_t, Interface == Mt6701Interface::I2C>; + using Base::base_mutex_; + using Base::count_; + using Base::logger_; + using Base::read; // BasePeripheral's buffer read, used by the SSI read() below + using Base::read_u8_from_register; + using Base::set_address; + using Base::set_read; + using Base::set_write; + + // Allow the base to invoke our (protected) read() via CRTP static dispatch. + friend Base; public: static constexpr uint8_t DEFAULT_ADDRESS = (0b0000110); ///< I2C address of the MT6701. It can be programmed to be 0b1000110 as well. ///< Only used if Interface == Mt6701Interface::I2C. - /** - * @brief Filter the input raw velocity and return it. - * @param raw Most recent raw velocity measured. - * @return Filtered velocity. - */ - typedef std::function velocity_filter_fn; + using typename Base::velocity_filter_fn; /** * @brief Enum class for the magnetic field strength of the MT6701. @@ -92,22 +103,6 @@ class Mt6701 : public BasePeripheral LOST = 1, ///< Tracking has been lost. }; - static constexpr int COUNTS_PER_REVOLUTION = - 16384; ///< Int number of counts per revolution for the magnetic encoder. - static constexpr float COUNTS_PER_REVOLUTION_F = - 16384.0f; ///< Float number of counts per revolution for the magnetic encoder. - static constexpr float COUNTS_TO_RADIANS = - 2.0f * (float)(M_PI) / - COUNTS_PER_REVOLUTION_F; ///< Conversion factor to convert from count value to radians. - static constexpr float COUNTS_TO_DEGREES = - 360.0f / - COUNTS_PER_REVOLUTION_F; ///< Conversion factor to convert from count value to degrees. - static constexpr float SECONDS_PER_MINUTE = - 60.0f; ///< Conversion factor to convert from seconds to minutes. - - static constexpr int MIN_DIFF = - CONFIG_MT6701_MIN_DIFF; ///< Minimum difference for velocity calculation. - /** * @brief Configuration information for the Mt6701. */ @@ -122,23 +117,21 @@ class Mt6701 : public BasePeripheral ///< called once every update_period seconds. std::chrono::duration update_period{ .01f}; ///< Update period (1/sample rate) in seconds. This determines the periodicity of the - ///< task which will read the position, update the accumulator, and update/filter + ///< timer which will read the position, update the accumulator, and update/filter ///< velocity. bool auto_init{true}; ///< Whether to automatically initialize the accumulator to the current ///< position on startup. - bool run_task{true}; ///< Whether to run the task/timer on startup. If + bool run_task{true}; ///< Whether to run the timer on startup. If ///< false, you must call update() manually. Logger::Verbosity log_level{Logger::Verbosity::WARN}; }; /** - * @brief Construct the Mt6701 and start the update task/timer if auto_init and run_task are true. + * @brief Construct the Mt6701 and start the update timer if auto_init and run_task are true. * @param config Configuration for the Mt6701. */ explicit Mt6701(const Config &config) - : BasePeripheral({}, "Mt6701", config.log_level) - , velocity_filter_(config.velocity_filter) - , update_period_(config.update_period) { + : Base("Mt6701", config.velocity_filter, config.update_period, config.log_level) { if constexpr (Interface == Mt6701Interface::I2C) { set_address(config.device_address); set_write(config.write); @@ -148,146 +141,12 @@ class Mt6701 : public BasePeripheral } if (config.auto_init) { std::error_code ec; - initialize(config.run_task, ec); + this->initialize(config.run_task, ec); } } -#if !defined(CONFIG_MT6701_USE_TIMER) || defined(_DOXYGEN_) - /** - * @brief Construct the Mt6701 and start the update task/timer if auto_init and run_task are true. - * @param config Configuration for the Mt6701. - * @param task_config Configuration for the internal task. - */ - explicit Mt6701(const Config &config, const espp::Task::Config &task_config) - : BasePeripheral({}, "Mt6701", config.log_level) - , velocity_filter_(config.velocity_filter) - , update_period_(config.update_period) - , task_(espp::Task::make_unique(task_config)) { - if constexpr (Interface == Mt6701Interface::I2C) { - set_address(config.device_address); - set_write(config.write); - set_read(config.read); - } else { - set_read(config.read); - } - if (config.auto_init) { - std::error_code ec; - initialize(config.run_task, ec); - } - } -#endif - - /** - * @brief Initialize the accumulator to the current position and start the - * update task. - * @param ec Error code to set if there is an error. - * @note This version of initialize() starts the update task, so you do not - * need to call update() manually. - */ - void initialize(std::error_code &ec) { initialize(true, ec); } - - /** - * @brief Initialize the accumulator to the current position and start the - * update task, if desired. - * @param run_task Whether to start the update task. - * @param ec Error code to set if there is an error. - * @note If you do not start the task, you must call update() manually. - */ - void initialize(bool run_task, std::error_code &ec) { - logger_.info("Initializing. Fastest measurable velocity will be {:.3f} RPM", - // half a rotation in one update period is the fastest we can - // measure - 0.5f / update_period_.count() * SECONDS_PER_MINUTE); - init(run_task, ec); - if (ec) { - logger_.error("Error initializing: {}", ec.message()); - } - } - -#if !defined(CONFIG_MT6701_USE_TIMER) || defined(_DOXYGEN_) - /** - * @brief Initialize the accumulator to the current position and start the - * update task, if desired. - * @param run_task Whether to start the update task. - * @param task_config Configuration for the internal task. - * @param ec Error code to set if there is an error. - * @note If you do not start the task, you must call update() manually. - */ - void initialize(bool run_task, const espp::Task::Config &task_config, std::error_code &ec) { - // create the task (discard any previous one) - task_.reset(); - task_ = espp::Task::make_unique(task_config); - initialize(run_task, ec); - } -#endif - - /** - * @brief Return whether the sensor needs to search for absolute 0 on startup. - * @note The MT6701 (using I2C/SPI) does not need to search for absolute 0 - * and will always know it on startup. Therefore this function always - * returns false. - * @return False because the magnetic sensor (using I2C/SPI) does not need to - * search for 0. - */ - bool needs_zero_search() const { return false; } - - /** - * @brief Get the most recently updated raw count value from the encoder. - * @note This value always represents the angle of the encoder modulo one - * rotation, meaning it only represents the range 0 to 360 degrees. It - * is not recommended to use this function, but is provided for edge use - * cases. - * @return Raw count value in the range [0, 16384] (0 to 360 degrees). - */ - int get_count() const { return count_.load(); } - - /** - * @brief Return the accumulated count that the encoder has generated since it - * was initialized. - * @note This value is a raw counter value that can be +/-, meaning - * COUNTS_PER_REVOLUTION can be used to convert it to revolutions. - * @return Raw accumulator value. - */ - int get_accumulator() const { return accumulator_.load(); } - - /** - * @brief Reset the accumulator to zero. - */ - void reset_accumulator() { accumulator_ = 0; } - - /** - * @brief Return the mechanical / shaft angle of the encoder, in radians, - * within the range [0, 2pi]. - * @return Angle in radians of the encoder within the range [0, 2pi]. - */ - float get_mechanical_radians() const { return (float)get_count() * COUNTS_TO_RADIANS; } - - /** - * @brief Return the mechanical / shaft angle of the encoder, in degrees, - * within the range [0, 360]. - * @return Angle in degrees of the encoder within the range [0, 360]. - */ - float get_mechanical_degrees() const { return (float)get_count() * COUNTS_TO_DEGREES; } - - /** - * @brief Return the accumulated position of the encoder, in radians. - * @note This can be any value, it is not restricted to [0, 2pi]. - * @return Position in radians of the encoder. - */ - float get_radians() const { return (float)get_accumulator() * COUNTS_TO_RADIANS; } - - /** - * @brief Return the accumulated position of the encoder, in degrees. - * @note This can be any value, it is not restricted to [0, 360]. - * @return Position in degrees of the encoder. - */ - float get_degrees() const { return (float)get_accumulator() * COUNTS_TO_DEGREES; } - - /** - * @brief Return the filtered velocity of the encoder, in RPM. - * @return Filtered velocity (revolutions / minute, RPM). - */ - float get_rpm() const { return velocity_rpm_.load(); } + /// @brief Stop the update timer before the object is destroyed. + ~Mt6701() { this->stop(); } /** * @brief Return the magnetic field strength of the encoder. @@ -317,107 +176,7 @@ class Mt6701 : public BasePeripheral return push_button_.load(); } - /** - * @brief Update the state of the encoder by reading the latest data from the - * encoder and updating the associated state. - * @param ec Error code to set if there is an error. - * @note You should not call this function if you have started the encoder's - * update task (e.g. run_task = true in the constructor, or you called - * initialize(true)). - */ - void update(std::error_code &ec) { - std::lock_guard lock(base_mutex_); - // measure update timing - uint64_t now_us = esp_timer_get_time(); - auto dt = now_us - prev_time_us_; - float seconds = dt / 1e6f; - prev_time_us_ = now_us; - // store the previous count - int prev_count = count_; - // read the latest data from the encoder and update the state - read(ec); - if (ec) { - return; - } - // compute diff - int diff = count_ - prev_count; - // check for zero crossing - if (diff > COUNTS_PER_REVOLUTION / 2) { - // we crossed zero going clockwise (1 -> 359) - diff -= COUNTS_PER_REVOLUTION; - } else if (diff < -COUNTS_PER_REVOLUTION / 2) { - // we crossed zero going counter-clockwise (359 -> 1) - diff += COUNTS_PER_REVOLUTION; - } - // update accumulator - accumulator_ += diff; - logger_.debug_rate_limited("CDA: {}, {}, {}", count_, diff, accumulator_); - // update velocity (filtering it) - float raw_velocity = - (dt > 0 && std::abs(diff) > MIN_DIFF) - ? (float)(diff) / COUNTS_PER_REVOLUTION_F / seconds * SECONDS_PER_MINUTE - : 0.0f; - velocity_rpm_ = velocity_filter_ ? velocity_filter_(raw_velocity) : raw_velocity; - if (dt > 0) { - float max_velocity = 0.5f / seconds * SECONDS_PER_MINUTE; - if (raw_velocity >= max_velocity) { - logger_.warn_rate_limited( - "Velocity nearing measurement limit ({:.3f} RPM), consider decreasing your " - "update period!", - max_velocity); - } - } - } - - /** - * @brief Start the update task/timer. - * @note This will start the task/timer that calls update() at the update_period. - * @note This is only useful if you previously stopped the task/timer or if you - * initialized with run_task = false. - * @return True if the task/timer was started successfully, false otherwise. - */ - bool start() { - logger_.info("Starting task with update period of {:.3f} seconds", update_period_.count()); - prev_time_us_ = esp_timer_get_time(); -#if defined(CONFIG_MT6701_USE_TIMER) - uint64_t period_us = - std::chrono::duration_cast(update_period_).count(); - return timer_.periodic(period_us); -#else - if (!task_) { - return false; - } - return task_->start(); -#endif - } - - /** - * @brief Stop the update task/timer. - * @note This will stop the task/timer that calls update() at the update_period. - * @note After stopping, you can manually call update() or restart with start(). - */ - void stop() { - logger_.info("Stopping task"); -#if defined(CONFIG_MT6701_USE_TIMER) - timer_.stop(); -#else - if (task_) { - task_->stop(); - } -#endif - } - protected: -#pragma pack(push, 1) - - struct MagneticFieldStatus { - uint8_t magnetic_field_strength : 2; - uint8_t push_button : 1; - uint8_t tracking_status : 1; - }; - -#pragma pack(pop) - void read(std::error_code &ec) requires(Interface == Mt6701Interface::I2C) { std::lock_guard lock(base_mutex_); // read the angle count registers @@ -443,18 +202,27 @@ class Mt6701 : public BasePeripheral logger_.error_rate_limited("Error reading: {}", ec.message()); return; } - // the first 14 bits is the angle data, followed by 4 bit status, and 6 bit + // the first 14 bits are the angle data, followed by 4 bit status, and 6 bit // crc uint16_t angle_h = buffer[0]; uint8_t angle_l = buffer[1] >> 2; + uint16_t raw_count = (angle_h << 6) | angle_l; // status is the lower 2 bits of the second byte and the upper 2 bits of // the third byte uint8_t status = ((buffer[1] & 0b11) << 2) | (buffer[2] >> 6); // crc is the lower 6 bits of the third byte uint8_t crc = buffer[2] & 0b111111; - logger_.debug("Angle: {}, Status: {}, CRC: {}", (angle_h << 8) | angle_l, status, crc); + // The CRC is computed over the 18 data bits (14-bit angle + 4-bit status). + // NOTE: this is currently observability-only (a mismatch is logged but the + // sample is still used) since the CRC implementation has not been verified + // against hardware; it can be promoted to rejecting the sample once verified. + uint8_t expected_crc = crc6((static_cast(raw_count) << 4) | (status & 0x0F)); + if (crc != expected_crc) { + logger_.warn_rate_limited("CRC mismatch: got {:#04x}, expected {:#04x}", crc, expected_crc); + } + logger_.debug("Angle: {}, Status: {}, CRC: {}", raw_count, status, crc); // update the count - count_ = ((angle_h << 6) | angle_l); + count_ = raw_count; // update the magnetic field strength, tracking status, and push button. // strength is the lower two bits [0:1], push button is the third bit [2], // and tracking status is the fourth bit [3] @@ -463,52 +231,20 @@ class Mt6701 : public BasePeripheral tracking_status_ = (TrackingStatus)((status >> 3) & 0b1); } -#if defined(CONFIG_MT6701_USE_TIMER) - bool update_task() { - std::error_code ec; - update(ec); - if (ec) { - logger_.error("Error updating: {}", ec.message()); - } - // don't want to stop the task - return false; - } -#else - bool update_task(std::mutex &m, std::condition_variable &cv, bool &task_notified) { - auto start_time = std::chrono::high_resolution_clock::now(); - std::error_code ec; - update(ec); - if (ec) { - logger_.error("Error updating: {}", ec.message()); - } - // sleep until the next update period - { - std::unique_lock lk(m); - cv.wait_until(lk, start_time + update_period_, [&task_notified] { return task_notified; }); - task_notified = false; - } - // don't want to stop the task - return false; - } -#endif - - void init(bool run_task, std::error_code &ec) { - std::lock_guard lock(base_mutex_); - // initialize the accumulator to have the current angle - read(ec); - if (ec) { - return; - } - accumulator_ = count_.load(); - if (!run_task) { - logger_.info( - "Not starting task, run_task is false. Manually call update() to update the state."); - return; - } - if (!start()) { - logger_.error("Error starting task"); - ec = make_error_code(std::errc::operation_not_permitted); - } + /// @brief MT6701 SSI CRC-6 (polynomial x^6 + x + 1) over the 18-bit payload. + /// @param data18 The 18-bit payload: (14-bit angle << 4) | 4-bit status. + /// @return The computed 6-bit CRC. + static uint8_t crc6(uint32_t data18) { + static constexpr uint8_t table[64] = { + 0x00, 0x03, 0x06, 0x05, 0x0C, 0x0F, 0x0A, 0x09, 0x18, 0x1B, 0x1E, 0x1D, 0x14, + 0x17, 0x12, 0x11, 0x30, 0x33, 0x36, 0x35, 0x3C, 0x3F, 0x3A, 0x39, 0x28, 0x2B, + 0x2E, 0x2D, 0x24, 0x27, 0x22, 0x21, 0x23, 0x20, 0x25, 0x26, 0x2F, 0x2C, 0x29, + 0x2A, 0x3B, 0x38, 0x3D, 0x3E, 0x37, 0x34, 0x31, 0x32, 0x13, 0x10, 0x15, 0x16, + 0x1F, 0x1C, 0x19, 0x1A, 0x0B, 0x08, 0x0D, 0x0E, 0x07, 0x04, 0x01, 0x02}; + uint8_t crc = table[(data18 >> 12) & 0x3F]; + crc = table[crc ^ ((data18 >> 6) & 0x3F)]; + crc = table[crc ^ (data18 & 0x3F)]; + return crc; } /** @@ -540,25 +276,9 @@ class Mt6701 : public BasePeripheral A_STOP_LOW = 0x40, ///< A_STOP[7:0] }; - velocity_filter_fn velocity_filter_{nullptr}; - uint64_t prev_time_us_{0}; - std::chrono::duration update_period_; - std::atomic count_{0}; - std::atomic accumulator_{0}; - std::atomic velocity_rpm_{0}; std::atomic magnetic_field_strength_{MagneticFieldStrength::NORMAL}; std::atomic tracking_status_{TrackingStatus::NORMAL}; std::atomic push_button_{false}; -#if defined(CONFIG_MT6701_USE_TIMER) - espp::HighResolutionTimer timer_{ - {.name = "Mt6701", - .callback = std::bind(&Mt6701::update_task, this) }}; -#else - std::unique_ptr task_ = espp::Task::make_unique(Task::Config{ - .callback = std::bind_front(&Mt6701::update_task, this), - .task_config = {.name = "Mt6701"}, - }); -#endif }; } // namespace espp diff --git a/doc/Doxyfile b/doc/Doxyfile index 0aa324ada9..b04dbd0f4e 100755 --- a/doc/Doxyfile +++ b/doc/Doxyfile @@ -337,6 +337,7 @@ INPUT = \ $(PROJECT_PATH)/components/lsm6dso/include/lsm6dso_detail.hpp \ $(PROJECT_PATH)/components/m5stack-cardputer/include/m5stack-cardputer.hpp \ $(PROJECT_PATH)/components/m5stack-tab5/include/m5stack-tab5.hpp \ + $(PROJECT_PATH)/components/magnetic_encoder/include/magnetic_encoder_base.hpp \ $(PROJECT_PATH)/components/math/include/bezier.hpp \ $(PROJECT_PATH)/components/math/include/fast_math.hpp \ $(PROJECT_PATH)/components/math/include/gaussian.hpp \ diff --git a/doc/en/encoder/as5600.rst b/doc/en/encoder/as5600.rst index c1ee4bcaeb..a7293e347c 100644 --- a/doc/en/encoder/as5600.rst +++ b/doc/en/encoder/as5600.rst @@ -12,9 +12,11 @@ measure * Accumulated degrees (since the component was created) * Speed (rotations per minute / RPM) -It does so by spawning a task which periodically reads the magnetic encoder, +It does so by spawning a timer which periodically reads the magnetic encoder, updates the accumulator, and computes the velocity. The component can be -configured to optionally filter the velocity. +configured to optionally filter the velocity. The timer can be either a +:cpp:class:`espp::HighResolutionTimer` (default) or an :cpp:class:`espp::Timer`, +selected via KConfig / menuconfig. The periodicity / update rate of the encoder can be configured at time of creation. diff --git a/doc/en/encoder/index.rst b/doc/en/encoder/index.rst index 9b752924a8..37dc2e638d 100644 --- a/doc/en/encoder/index.rst +++ b/doc/en/encoder/index.rst @@ -6,6 +6,7 @@ Encoder APIs abi_encoder encoder_types + magnetic_encoder as5600 mt6701 diff --git a/doc/en/encoder/magnetic_encoder.rst b/doc/en/encoder/magnetic_encoder.rst new file mode 100644 index 0000000000..eabcd135c3 --- /dev/null +++ b/doc/en/encoder/magnetic_encoder.rst @@ -0,0 +1,31 @@ +Magnetic Encoder Base +********************* + +The ``MagneticEncoderBase`` is the shared CRTP base class for the magnetic +angle encoders (:doc:`as5600` and :doc:`mt6701`). It holds all of the machinery +common to those encoders: + +* the periodic update loop (raw-count accumulation + velocity estimation), +* the position / velocity accessors (count, radians, degrees, accumulator, RPM), +* and the periodic driver that calls ``update()`` at the configured rate. + +The concrete encoder supplies exactly one thing - a ``read()`` that refreshes +the raw count from the sensor - which the base invokes through static (CRTP) +dispatch, so there is no virtual-call overhead even when the update loop runs at +very high frequency (e.g. 1-2 kHz). + +The periodic driver is selected at compile time (per encoder, via KConfig / +menuconfig): + +* a :cpp:class:`espp::HighResolutionTimer` (default), which has microsecond + resolution and is the right choice for sub-millisecond update periods, or +* an :cpp:class:`espp::Timer`, which schedules against an absolute wake-up time + (drift-free and in phase - important for stable velocity / accumulator state) + but is limited to the FreeRTOS tick resolution. + +.. ---------------------------- API Reference ---------------------------------- + +API Reference +------------- + +.. include-build-file:: inc/magnetic_encoder_base.inc diff --git a/doc/en/encoder/mt6701.rst b/doc/en/encoder/mt6701.rst index d0be1ab10b..54a28bff70 100644 --- a/doc/en/encoder/mt6701.rst +++ b/doc/en/encoder/mt6701.rst @@ -12,9 +12,11 @@ measure * Accumulated degrees (since the component was created) * Speed (rotations per minute / RPM) -It does so by spawning a task which periodically reads the magnetic encoder, +It does so by spawning a timer which periodically reads the magnetic encoder, updates the accumulator, and computes the velocity. The component can be -configured to optionally filter the velocity. +configured to optionally filter the velocity. The timer can be either a +:cpp:class:`espp::HighResolutionTimer` (default) or an :cpp:class:`espp::Timer`, +selected via KConfig / menuconfig. The periodicity / update rate of the encoder can be configured at time of creation.