Maneet/active diff cpp

firmware/hexray/VC/CMakeLists.txt

@@ -12,11 +12,11 @@ set(SYSTEM_INCLUDE_DIRS
         #        "${CMAKE_CURRENT_SOURCE_DIR}/boot"
 )
 
-file(GLOB_RECURSE APP_SRCS CONFIGURE_DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/src/app/*.c")
+file(GLOB_RECURSE APP_SRCS CONFIGURE_DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/src/app/*.c" "${CMAKE_CURRENT_SOURCE_DIR}/src/app/*.cpp")
 list(APPEND APP_SRCS)
 set(APP_INCLUDE_DIRS
         # "${CMAKE_CURRENT_SOURCE_DIR}/src/app"
-        "${SHARED_APP_INCLUDE_DIR}" "${SHARED_APP_INCLUDE_DIR_CPP}"
+        "${SHARED_APP_INCLUDE_DIR}" "${SHARED_APP_INCLUDE_DIR_CPP}" "${CMAKE_CURRENT_SOURCE_DIR}/src/app/torque_vectoring/controllers/yaw_rate_control" "${CMAKE_CURRENT_SOURCE_DIR}/src/app/torque_vectoring/controllers" "${CMAKE_CURRENT_SOURCE_DIR}/src/app/torque_vectoring/datatypes" "${CMAKE_CURRENT_SOURCE_DIR}/src/app/torque_vectoring"
 )
 
 file(GLOB_RECURSE IO_SRCS CONFIGURE_DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/src/io/*.c")
@@ -99,7 +99,7 @@ elseif ("${TARGET}" STREQUAL "test")
         )
     set(INCLUDE_DIRS
             # "${CMAKE_CURRENT_SOURCE_DIR}/test"
-            ${APP_INCLUDE_DIRS} ${IO_INCLUDE_DIRS} ${TEST_INCLUDE_DIRS} ${SYSTEM_INCLUDE_DIRS} ${SHARED_UTIL_INCLUDE_DIR_CPP}
+            ${APP_INCLUDE_DIRS} ${IO_INCLUDE_DIRS} ${TEST_INCLUDE_DIRS} ${SYSTEM_INCLUDE_DIRS} ${SHARED_UTIL_INCLUDE_DIR_CPP} ${SHARED_UTIL_INCLUDE_DIR}
     )
     compile_gtest_executable(
             "hexray_VC_test"

firmware/hexray/VC/src/app/torque_vectoring/app_pid.c

@@ -0,0 +1,96 @@
+#include "app_pid.h"
+#include "app_utils.h"
+#include <assert.h>
+
+void app_pid_init(PID *pid, const PID_Config *conf)
+{
+    pid->Kp               = conf->Kp;
+    pid->Ki               = conf->Ki;
+    pid->Kd               = conf->Kd;
+    pid->Kb               = conf->Kb;
+    pid->Kff              = conf->Kff;
+    pid->out_min          = conf->out_min;
+    pid->out_max          = conf->out_max;
+    pid->smoothing_coeff  = conf->smoothing_coeff;
+    pid->max_integral     = conf->max_integral;
+    pid->min_integral     = conf->min_integral;
+    pid->clamp_integral   = conf->clamp_integral;
+    pid->clamp_output     = conf->clamp_output;
+    pid->back_calculation = conf->back_calculation;
+    pid->feed_forward     = conf->feed_forward;
+    pid->sample_time      = conf->sample_time;
+
+    pid->prev_error       = 0.0f;
+    pid->prev_derivative  = 0.0f;
+    pid->prev_disturbance = 0.0f;
+    pid->derivative       = 0.0f;
+    pid->integral         = 0.0f;
+    pid->error            = 0.0f;
+
+    assert(pid->out_max >= pid->out_min);
+    assert(pid->max_integral >= pid->min_integral);
+    assert(pid->sample_time > 0.0f);
+}
+
+/**
+ * PID controller effort implementation with derivative-on-error and discrete time handling. Make sure to Scale
+ * your inputs accordingly.Includes:
+ * - Integral Anti-wind up in the form of clamping and optional back calculation
+ * - Derivative Filtering in the form of first order exponential smoothing
+ * - Static Gain feed forward model
+ * @param pid data store
+ * @param setpoint setpoint
+ * @param input also "measurement"/"process variable"
+ * @param disturbance feedforward/"disturbance" variable
+ * @return controller output/"effort"
+ */
+float app_pid_compute(PID *pid, const float setpoint, const float input, float disturbance)
+{
+    pid->error = setpoint - input;
+
+    pid->integral += pid->error * pid->sample_time;
+
+    // First order exponential smoothing on derivative (https://en.wikipedia.org/wiki/Exponential_smoothing)
+    float raw_derivative = (pid->error - pid->prev_error) / pid->sample_time;
+    float derivative     = pid->smoothing_coeff * raw_derivative + (1.0f - pid->smoothing_coeff) * pid->prev_derivative;
+
+    // Feed Forward
+    float u_ff = 0.0f;
+    if (pid->feed_forward)
+    {
+        u_ff = pid->Kff * disturbance;
+    }
+
+    float output = pid->Kp * pid->error + pid->Ki * pid->integral + pid->Kd * derivative + u_ff;
+
+    // Anti-Windup with back calculation (https://www.cds.caltech.edu/~murray/books/AM08/pdf/am08-pid_04Mar10.pdf)
+    // equation 10.16
+    if (pid->back_calculation)
+    {
+        float out_clamp = CLAMP(output, pid->out_min, pid->out_max);
+        pid->integral += pid->Kb * (out_clamp - output);
+    }
+
+    if (pid->clamp_output)
+    {
+        output = CLAMP(output, pid->out_min, pid->out_max);
+    }
+    // Conditional Anti-Windup
+    if (pid->clamp_integral)
+    {
+        pid->integral = CLAMP(pid->integral, pid->min_integral, pid->max_integral);
+    }
+    pid->prev_error       = pid->error;
+    pid->prev_disturbance = disturbance;
+    pid->prev_derivative  = derivative;
+
+    return output;
+}
+
+void app_pid_requestReset(PID *pid)
+{
+    pid->integral         = 0.0f;
+    pid->prev_derivative  = 0.0f;
+    pid->prev_disturbance = 0.0f;
+    pid->prev_error       = 0.0f;
+}

firmware/hexray/VC/src/app/torque_vectoring/app_pid.cpp

@@ -0,0 +1,104 @@
+#include "app_pid.hpp"
+#include <stdexcept>
+#include <assert.h>
+
+namespace app
+{
+PID::PID(const Config &conf)
+  : Kp(conf.Kp),
+    Ki(conf.Ki),
+    Kd(conf.Kd),
+    Kb(conf.Kb),
+    Kff(conf.Kff),
+    smoothing_coeff(conf.smoothing_coeff),
+    out_max(conf.out_max),
+    out_min(conf.out_min),
+    max_integral(conf.max_integral),
+    min_integral(conf.min_integral),
+    clamp_output(conf.clamp_output),
+    clamp_integral(conf.clamp_integral),
+    back_calculation(conf.back_calculation),
+    feed_forward(conf.feed_forward),
+    sample_time(conf.sample_time)
+{
+    assert(out_max >= out_min);
+    assert(max_integral >= min_integral);
+    assert(sample_time > 0.0f);
+}
+
+/**
+ * PID controller effort implementation with derivative-on-error and discrete time handling. Includes:
+ * - Integral anti-wind up in the form of clamping and optional back calculation (requires valid min and max output
+ * values)
+ * - Derivative Filtering in the form of first order exponential smoothing
+ * - Static Gain feed forward model
+ * @param setpoint setpoint
+ * @param input also "measurement"/"process variable"
+ * @param disturbance feedforward/"disturbance" variable
+ * @return controller output/"effort"
+ */
+
+float PID::compute(float setpoint, float input, float disturbance = 0.0f)
+{
+    error = setpoint - input;
+
+    integral += error * sample_time;
+
+    // First order exponential smoothing (https://en.wikipedia.org/wiki/Exponential_smoothing)
+    float raw_derivative = (error - prev_error) / sample_time;
+    derivative           = smoothing_coeff * raw_derivative + (1.0f - smoothing_coeff) * prev_derivative;
+    derivative           = smoothing_coeff * raw_derivative + (1.0f - smoothing_coeff) * prev_derivative;
+
+    // Feed Forward
+    float u_ff = 0.0f;
+    if (feed_forward)
+    {
+        u_ff = Kff * disturbance;
+    }
+
+    float output = Kp * error + Ki * integral + Kd * derivative + u_ff;
+
+    // Anti-Windup with back calculation (https://www.cds.caltech.edu/~murray/books/AM08/pdf/am08-pid_04Mar10.pdf)
+    // equation 10.16
+    if (back_calculation)
+    {
+        float out_clamp = std::clamp(output, out_min, out_max);
+        integral += Kb * (out_clamp - output);
+    }
+
+    if (clamp_output)
+    {
+        output = std::clamp(output, out_min, out_max);
+    }
+    // Conditional Anti-Windup
+    if (clamp_integral)
+    {
+        integral = std::clamp(integral, min_integral, max_integral);
+    }
+
+    prev_error       = error;
+    prev_disturbance = disturbance;
+    prev_derivative  = derivative;
+
+    return output;
+}
+
+void PID::reset()
+{
+    integral         = 0.0f;
+    prev_derivative  = 0.0f;
+    prev_disturbance = 0.0f;
+    prev_error       = 0.0f;
+}
+
+float PID::getIntegral()
+{
+    return integral;
+}
+
+float PID::getDerivative()
+{
+    return derivative;
+}
+
+} // namespace app

firmware/hexray/VC/src/app/torque_vectoring/app_pid.h

@@ -0,0 +1,61 @@
+#pragma once
+#include <stdint.h>
+#include <stdbool.h>
+
+typedef struct
+{
+    // Gains and Coefficients
+    float Kp;
+    float Ki;
+    float Kd;
+    float Kb;
+    float Kff; // often the inverse of Kp
+    float smoothing_coeff;
+
+    // Limits
+    float out_max;
+    float out_min;
+    float max_integral;
+    float min_integral;
+
+    // Internal State
+    float error;
+    float integral;
+    float derivative;
+    float prev_derivative;
+    float prev_disturbance;
+    float prev_error;
+
+    bool  clamp_output;
+    bool  clamp_integral;
+    bool  back_calculation;
+    bool  feed_forward;
+    float sample_time;
+} PID;
+
+typedef struct
+{
+    const float Kp;
+    const float Ki;
+    const float Kd;
+    const float Kb;
+
+    const float Kff;
+    const float smoothing_coeff;
+
+    const float out_max;
+    const float out_min;
+    const float max_integral;
+    const float min_integral;
+
+    const bool  clamp_output;
+    const bool  clamp_integral;
+    const bool  back_calculation;
+    const bool  feed_forward;
+    const float sample_time;
+} PID_Config;
+
+void app_pid_init(PID *pid, const PID_Config *conf);
+
+float app_pid_compute(PID *pid, float setpoint, float input, float disturbance);
+void  app_pid_requestReset(PID *pid);

firmware/hexray/VC/src/app/torque_vectoring/app_pid.hpp

@@ -0,0 +1,68 @@
+#pragma once
+#include <cstdint>
+#include <algorithm>
+
+namespace app
+{
+class PID
+{
+  public:
+    struct Config
+    {
+        float Kp;
+        float Ki;
+        float Kd;
+        float Kb;
+
+        float Kff;
+        float smoothing_coeff;
+
+        float out_max;
+        float out_min;
+        float max_integral;
+        float min_integral;
+
+        bool  clamp_output;
+        bool  clamp_integral;
+        bool  back_calculation;
+        bool  feed_forward;
+        float sample_time;
+    };
+
+    explicit PID(const Config &conf);
+
+    float compute(float setpoint, float input, float disturbance);
+    void  reset();
+    float getIntegral();
+    float getDerivative();
+
+  private:
+    // Gains and coefficients
+    float Kp;
+    float Ki;
+    float Kd;
+    float Kb;
+    float Kff;             // often the inverse of Kp
+    float smoothing_coeff; // set to 1 for no smoothing
+
+    // Limits
+    float out_max;
+    float out_min;
+    float max_integral;
+    float min_integral;
+
+    // Internal state
+    float error            = 0.0f;
+    float integral         = 0.0f;
+    float derivative       = 0.0f;
+    float prev_derivative  = 0.0f;
+    float prev_disturbance = 0.0f;
+    float prev_error       = 0.0f;
+
+    bool  clamp_output;
+    bool  clamp_integral;
+    bool  back_calculation;
+    bool  feed_forward;
+    float sample_time;
+};
+} // namespace app