feat(adrc): Add Active Disturbance Rejection Control (ADRC) component

.github/workflows/build.yml

@@ -21,6 +21,8 @@ jobs:
         test:
         - path: 'components/adc/example'
           target: esp32
+        - path: 'components/adrc/example'
+          target: esp32
         - path: 'components/ads1x15/example'
           target: esp32
         - path: 'components/ads7138/example'

.github/workflows/upload_components.yml

@@ -28,6 +28,7 @@ jobs:
           #
           components: |
             components/adc
+            components/adrc
             components/ads1x15
             components/ads7138
             components/adxl345

components/adrc/CMakeLists.txt

@@ -0,0 +1,4 @@
+idf_component_register(
+  INCLUDE_DIRS "include"
+  SRC_DIRS "src"
+  REQUIRES base_component)

components/adrc/README.md

@@ -0,0 +1,21 @@
+# ADRC (Active Disturbance Rejection Control) Component
+
+[![Badge](https://components.espressif.com/components/espp/adrc/badge.svg)](https://components.espressif.com/components/espp/adrc)
+
+The `adrc` component provides reusable active disturbance rejection control
+implementations for ESPP applications.
+
+## Features
+
+- Linear first-order ADRC
+- Linear second-order ADRC
+- Han-style nonlinear first-order ADRC
+- Han-style nonlinear second-order ADRC
+- Han tracking differentiator utility for smoothing references and estimating
+  reference rate
+- Thread-safe configuration and state updates
+
+## Example
+
+The [example](./example) shows how to use the ADRC classes against simulated
+first-order and second-order plants with injected disturbances.

components/adrc/example/CMakeLists.txt

@@ -0,0 +1,21 @@
+# The following lines of boilerplate have to be in your project's CMakeLists
+# in this exact order for cmake to work correctly
+cmake_minimum_required(VERSION 3.20)
+
+set(ENV{IDF_COMPONENT_MANAGER} "0")
+include($ENV{IDF_PATH}/tools/cmake/project.cmake)
+
+set(EXTRA_COMPONENT_DIRS
+  "../../../components/"
+)
+
+set(
+  COMPONENTS
+  "main esptool_py adrc"
+  CACHE STRING
+  "List of components to include"
+  )
+
+project(adrc_example)
+
+set(CMAKE_CXX_STANDARD 20)

components/adrc/example/README.md

@@ -0,0 +1,24 @@
+# ADRC Example
+
+This example shows how to use the `espp::LinearAdrcFirstOrder`,
+`espp::LinearAdrcSecondOrder`, `espp::HanAdrcFirstOrder`, and
+`espp::HanAdrcSecondOrder` classes against simulated plants with injected
+disturbances.
+
+## How to use example
+
+### Build and Flash
+
+Build the project and flash it to the target, then run monitor tool to view
+serial output:
+
+```
+idf.py -p PORT flash monitor
+```
+
+(Replace PORT with the name of the serial port to use.)
+
+(To exit the serial monitor, type ``Ctrl-]``.)
+
+See the Getting Started Guide for full steps to configure and use ESP-IDF to
+build projects.

components/adrc/example/main/CMakeLists.txt

@@ -0,0 +1,2 @@
+idf_component_register(SRC_DIRS "."
+                       INCLUDE_DIRS ".")

components/adrc/example/main/adrc_example.cpp

@@ -0,0 +1,174 @@
+#include <chrono>
+#include <thread>
+
+#include "adrc.hpp"
+
+using namespace std::chrono_literals;
+
+namespace {
+struct FirstOrderPlant {
+  float pole{-1.2f};
+  float gain{1.0f};
+  float disturbance{0.0f};
+  float y{0.0f};
+
+  float update(float input, float dt) {
+    y += dt * (pole * y + gain * input + disturbance);
+    return y;
+  }
+};
+
+struct SecondOrderPlant {
+  float natural_frequency{3.5f};
+  float damping_ratio{0.45f};
+  float gain{1.0f};
+  float disturbance{0.0f};
+  float x1{0.0f};
+  float x2{0.0f};
+
+  float update(float input, float dt) {
+    auto acceleration = -2.0f * damping_ratio * natural_frequency * x2 -
+                        natural_frequency * natural_frequency * x1 + gain * input + disturbance;
+    x1 += dt * x2;
+    x2 += dt * acceleration;
+    return x1;
+  }
+};
+} // namespace
+
+extern "C" void app_main(void) {
+  constexpr float dt = 0.001f;
+  constexpr int num_steps = 5000;
+  constexpr int print_interval = 100;
+  fmt::print("ADRC example\n");
+
+  {
+    fmt::print("Linear first-order ADRC example\n");
+    //! [adrc linear first order example]
+    espp::LinearAdrcFirstOrder controller({
+        .b0 = 1.0f,
+        .controller_bandwidth = 8.0f,
+        .observer_bandwidth = 24.0f,
+        .output_min = -4.0f,
+        .output_max = 4.0f,
+    });
+    FirstOrderPlant plant;
+    for (int i = 0; i < num_steps; ++i) {
+      auto time = i * dt;
+      auto reference = time >= 0.25f ? 1.0f : 0.0f;
+      plant.disturbance = time >= 2.5f ? -0.8f : 0.0f;
+      auto control = controller.update(reference, plant.y, dt);
+      auto output = plant.update(control, dt);
+      if (i % print_interval == 0 || i == num_steps - 1) {
+        auto state = controller.get_state();
+        fmt::print("t={:0.2f}s ref={:0.2f} y={:0.3f} u={:0.3f} z2={:0.3f}\n", time, reference,
+                   output, state.output, state.z2);
+      }
+    }
+    //! [adrc linear first order example]
+  }
+
+  {
+    fmt::print("Linear second-order ADRC example\n");
+    //! [adrc linear second order example]
+    espp::LinearAdrcSecondOrder controller({
+        .b0 = 1.0f,
+        .controller_bandwidth = 10.0f,
+        .observer_bandwidth = 36.0f,
+        .output_min = -8.0f,
+        .output_max = 8.0f,
+    });
+    SecondOrderPlant plant;
+    for (int i = 0; i < num_steps; ++i) {
+      auto time = i * dt;
+      auto reference = time >= 0.25f ? 1.0f : 0.0f;
+      plant.disturbance = time >= 2.5f ? 1.2f : 0.0f;
+      auto control = controller.update(reference, plant.x1, dt);
+      auto output = plant.update(control, dt);
+      if (i % print_interval == 0 || i == num_steps - 1) {
+        auto state = controller.get_state();
+        fmt::print("t={:0.2f}s ref={:0.2f} y={:0.3f} u={:0.3f} z3={:0.3f}\n", time, reference,
+                   output, state.output, state.z3);
+      }
+    }
+    //! [adrc linear second order example]
+  }
+
+  {
+    fmt::print("Han first-order ADRC example\n");
+    //! [adrc han first order example]
+    espp::HanAdrcFirstOrder controller({
+        .b0 = 1.0f,
+        .controller_gain = 7.0f,
+        .observer_bandwidth = 22.0f,
+        .observer_alpha = 0.5f,
+        .controller_alpha = 0.8f,
+        .fal_delta = 0.01f,
+        .use_tracking_differentiator = true,
+        .tracking_config =
+            {
+                .tracking_bandwidth = 45.0f,
+                .filter_factor = 5.0f,
+            },
+        .output_min = -4.0f,
+        .output_max = 4.0f,
+    });
+    FirstOrderPlant plant;
+    for (int i = 0; i < num_steps; ++i) {
+      auto time = i * dt;
+      auto reference = time >= 0.25f ? 1.0f : 0.0f;
+      plant.disturbance = time >= 2.5f ? -0.8f : 0.0f;
+      auto control = controller.update(reference, plant.y, dt);
+      auto output = plant.update(control, dt);
+      if (i % print_interval == 0 || i == num_steps - 1) {
+        auto state = controller.get_state();
+        fmt::print("t={:0.2f}s ref={:0.2f} td={:0.3f} y={:0.3f} u={:0.3f}\n", time, reference,
+                   state.td_reference, output, state.output);
+      }
+    }
+    //! [adrc han first order example]
+  }
+
+  {
+    fmt::print("Han second-order ADRC example\n");
+    //! [adrc han second order example]
+    espp::HanAdrcSecondOrder controller({
+        .b0 = 1.0f,
+        .position_gain = 30.0f,
+        .rate_gain = 6.0f,
+        .observer_bandwidth = 32.0f,
+        .observer_alpha1 = 0.5f,
+        .observer_alpha2 = 0.25f,
+        .controller_alpha1 = 0.8f,
+        .controller_alpha2 = 1.5f,
+        .fal_delta = 0.01f,
+        .use_tracking_differentiator = true,
+        .tracking_config =
+            {
+                .tracking_bandwidth = 60.0f,
+                .filter_factor = 5.0f,
+            },
+        .output_min = -8.0f,
+        .output_max = 8.0f,
+    });
+    SecondOrderPlant plant;
+    for (int i = 0; i < num_steps; ++i) {
+      auto time = i * dt;
+      auto reference = time >= 0.25f ? 1.0f : 0.0f;
+      plant.disturbance = time >= 2.5f ? 1.2f : 0.0f;
+      auto control = controller.update(reference, plant.x1, dt);
+      auto output = plant.update(control, dt);
+      if (i % print_interval == 0 || i == num_steps - 1) {
+        auto state = controller.get_state();
+        fmt::print("t={:0.2f}s ref={:0.2f} td={:0.3f} y={:0.3f} u={:0.3f} z3={:0.3f}\n", time,
+                   reference, state.td_reference, output, state.output, state.z3);
+      }
+    }
+    //! [adrc han second order example]
+  }
+
+  fmt::print("ADRC example complete!\n");
+  while (true) {
+    std::this_thread::sleep_for(1s);
+  }
+}

components/adrc/example/sdkconfig.defaults

@@ -0,0 +1,4 @@
+# Common ESP-related
+#
+CONFIG_ESP_SYSTEM_EVENT_TASK_STACK_SIZE=4096
+CONFIG_ESP_MAIN_TASK_STACK_SIZE=8192

components/adrc/idf_component.yml

@@ -0,0 +1,20 @@
+## IDF Component Manager Manifest File
+license: "MIT"
+description: "Active disturbance rejection control (ADRC) component for ESP-IDF"
+url: "https://github.com/esp-cpp/espp/tree/main/components/adrc"
+repository: "git://github.com/esp-cpp/espp.git"
+maintainers:
+  - William Emfinger <waemfinger@gmail.com>
+documentation: "https://esp-cpp.github.io/espp/adrc.html"
+examples:
+  - path: example
+tags:
+  - cpp
+  - Component
+  - Control
+  - ADRC
+  - Math
+dependencies:
+  idf:
+    version: ">=5.0"
+  espp/base_component: ">=1.0"