#include "var_pid.h"

float Variable_KP(float err, transform_pid_conf_t conf)
{
    return conf.KP_a + conf.KP_b * (1 - expf(-conf.KP_c * fabsf(err)));
}

float Variable_KI(float err, transform_pid_conf_t conf)
{
    return conf.KI_a * expf(-conf.KI_c * fabsf(err));
}

float Variable_KI_Gain(float err, transform_pid_conf_t conf)
{
    if (fabsf(err) > conf.KI_e)
    {
        return 1;
    }
    else
    {
        return conf.KI_1 * expf(-conf.KI_0 * fabsf(err));
    }
}

float Variable_KD(float err, transform_pid_conf_t conf)
{
    if (conf.KD_a <= conf.KD_b)
    {
        return 0;
    }

    return conf.KD_a - conf.KD_b * (1 - expf(-conf.KD_c * fabsf(err)));
}