test/pid_expert.c

   1 #include "pid_expert.h"
   2 #if A_PREREQ_GNUC(3, 0) || __has_warning("-Wfloat-equal")
   3 #pragma GCC diagnostic ignored "-Wfloat-equal"
   4 #endif /* -Wfloat-equal */
   5
   6 void a_pid_expert_kpid(a_pid_expert *ctx, a_float kp, a_float ki, a_float kd)
   7 {
   8     a_pid_kpid(&ctx->pid, kp, ki, kd);
   9 }
  10
  11 a_float a_pid_expert_iter(a_pid_expert *ctx, a_float set, a_float fdb)
  12 {
  13     a_float out = 0;
  14     a_float const err = set - fdb;
  15     a_float const abs = A_ABS(err);
  16     a_float const var = ctx->pid.fdb - fdb;
  17     a_float const ec = err - ctx->pid.err;
  18
  19     if (-ctx->max1 > err)
  20     {
  21         out = -ctx->outmax;
  22         goto out;
  23     }
  24     else if (err > ctx->max1)
  25     {
  26         out = ctx->outmax;
  27         goto out;
  28     }
  29
  30     if (err * ec > 0 || ec == 0)
  31     {
  32         out = ctx->pid.kp * ec + ctx->pid.ki * err + ctx->pid.kd * (var - ctx->pid.var);
  33         if (abs > ctx->max2) { out *= ctx->gain; }
  34     }
  35     else if (ec * ctx->ec < 0 && err != 0)
  36     {
  37         out = ctx->pid.kp * ctx->pid.err;
  38         if (abs > ctx->max2) { out *= ctx->gain; }
  39         else { out *= ctx->loss; }
  40     }
  41
  42     if (abs < ctx->epsilon) { out = ctx->pid.kp * ec + ctx->pid.ki * err; }
  43
  44 out:
  45     ctx->pid.out = A_SAT(out, ctx->pid.outmin, ctx->pid.outmax);
  46     ctx->pid.fdb = fdb;
  47     ctx->pid.var = var;
  48     ctx->pid.err = err;
  49     ctx->ec = ec;
  50     return ctx->pid.out;
  51 }
  52
  53 void a_pid_expert_zero(a_pid_expert *ctx)
  54 {
  55     a_pid_zero(&ctx->pid);
  56     ctx->ec = 0;
  57 }