Nonlinear.hpp

Go to the documentation of this file.

#pragma once
 
#include <janus/janus.hpp>
 
namespace vulcan::tf {

template <typename Scalar>
Scalar rate_limit(const Scalar &current, const Scalar &commanded,
                  double rate_max, double dt) {
    Scalar delta = commanded - current;
    Scalar max_delta = static_cast<Scalar>(rate_max * dt);
    return current + janus::clamp(delta, -max_delta, max_delta);
}

template <typename Scalar>
Scalar saturate(const Scalar &value, const Scalar &min, const Scalar &max) {
    return janus::clamp(value, min, max);
}

template <typename Scalar>
Scalar deadband(const Scalar &input, double deadband_width) {
    Scalar db = static_cast<Scalar>(deadband_width);
    Scalar sign_input = janus::where(input < Scalar(0), Scalar(-1), Scalar(1));
    Scalar abs_input = janus::where(input < Scalar(0), -input, input);
 
    return janus::where(abs_input <= db, Scalar(0),
                        sign_input * (abs_input - db));
}

template <typename Scalar>
Scalar hysteresis(const Scalar &input, const Scalar &last_output,
                  double hysteresis_width) {
    Scalar h = static_cast<Scalar>(hysteresis_width);
    Scalar upper = last_output + h;
    Scalar lower = last_output - h;
 
    return janus::where(input > upper, input - h,
                        janus::where(input < lower, input + h, last_output));
}
 
} // namespace vulcan::tf