modules/devices/control_system/filters/pid/pid.c

   1 /*
   2  *  Copyright Droids Corporation, Microb Technology, Eirbot (2006)
   3  *
   4  *  This program is free software; you can redistribute it and/or modify
   5  *  it under the terms of the GNU General Public License as published by
   6  *  the Free Software Foundation; either version 2 of the License, or
   7  *  (at your option) any later version.
   8  *
   9  *  This program is distributed in the hope that it will be useful,
  10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  *  GNU General Public License for more details.
  13  *
  14  *  You should have received a copy of the GNU General Public License
  15  *  along with this program; if not, write to the Free Software
  16  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  17  *
  18  *  Revision : $Id: pid.c,v 1.5.4.10 2009-03-05 22:02:55 zer0 Exp $
  19  *
  20  */
  21
  22 #include <string.h>
  23 #include <pid.h>
  24
  25
  26 /** this function will initialize all fieds of pid structure to 0 */
  27 void pid_init(struct pid_filter *p)
  28 {
  29         uint8_t flags;
  30         IRQ_LOCK(flags);
  31         memset(p, 0, sizeof(*p));
  32         p->gain_P = 1 ;
  33         p->derivate_nb_samples = 1;
  34         IRQ_UNLOCK(flags);
  35 }
  36
  37 /** this function will initialize all fieds of pid structure to 0,
  38  *  except configuration */
  39 void pid_reset(struct pid_filter *p)
  40 {
  41         uint8_t flags;
  42         IRQ_LOCK(flags);
  43         memset(p->prev_samples, 0, sizeof(p->prev_samples));
  44         p->integral = 0;
  45         p->prev_D = 0;
  46         p->prev_out = 0;
  47         IRQ_UNLOCK(flags);
  48 }
  49
  50 void pid_set_gains(struct pid_filter *p, int16_t gp, int16_t gi, int16_t gd)
  51 {
  52         uint8_t flags;
  53         IRQ_LOCK(flags);
  54         p->gain_P  = gp;
  55         p->gain_I  = gi;
  56         p->gain_D  = gd;
  57         IRQ_UNLOCK(flags);
  58 }
  59
  60 void pid_set_maximums(struct pid_filter *p, int32_t max_in, int32_t max_I, int32_t max_out)
  61 {
  62         uint8_t flags;
  63         IRQ_LOCK(flags);
  64         p->max_in  = max_in;
  65         p->max_I   = max_I;
  66         p->max_out = max_out;
  67         IRQ_UNLOCK(flags);
  68 }
  69
  70 void pid_set_out_shift(struct pid_filter *p, uint8_t out_shift)
  71 {
  72         uint8_t flags;
  73         IRQ_LOCK(flags);
  74         p->out_shift=out_shift;
  75         IRQ_UNLOCK(flags);
  76 }
  77
  78 int8_t pid_set_derivate_filter(struct pid_filter *p, uint8_t nb_samples)
  79 {
  80         uint8_t flags;
  81         int8_t ret;
  82         IRQ_LOCK(flags);
  83         if (nb_samples > PID_DERIVATE_FILTER_MAX_SIZE) {
  84                 ret = -1;
  85         }
  86         else {
  87                 p->derivate_nb_samples = nb_samples;
  88                 ret = 0;
  89         }
  90         IRQ_UNLOCK(flags);
  91         return ret;
  92 }
  93
  94 int16_t pid_get_gain_P(struct pid_filter *p)
  95 {
  96         return (p->gain_P);
  97 }
  98
  99 int16_t pid_get_gain_I(struct pid_filter *p)
 100 {
 101         return (p->gain_I);
 102 }
 103
 104 int16_t pid_get_gain_D(struct pid_filter *p)
 105 {
 106         return (p->gain_D);
 107 }
 108
 109
 110 int32_t pid_get_max_in(struct pid_filter *p)
 111 {
 112         return (p->max_in);
 113 }
 114
 115 int32_t pid_get_max_I(struct pid_filter *p)
 116 {
 117         return (p->max_I);
 118 }
 119
 120 int32_t pid_get_max_out(struct pid_filter *p)
 121 {
 122         return (p->max_out);
 123 }
 124
 125
 126 uint8_t pid_get_out_shift(struct pid_filter *p)
 127 {
 128         return (p->out_shift);
 129 }
 130
 131 uint8_t pid_get_derivate_filter(struct pid_filter *p)
 132 {
 133         return (p->derivate_nb_samples);
 134 }
 135
 136 int32_t pid_get_value_I(struct pid_filter *p)
 137 {
 138         uint8_t flags;
 139         int32_t ret;
 140         IRQ_LOCK(flags);
 141         ret = (p->integral);
 142         IRQ_UNLOCK(flags);
 143         return ret;
 144 }
 145
 146 int32_t pid_get_value_in(struct pid_filter *p)
 147 {
 148         uint8_t flags;
 149         int32_t ret;
 150         IRQ_LOCK(flags);
 151         ret = p->prev_samples[p->index];
 152         IRQ_UNLOCK(flags);
 153         return ret;
 154 }
 155
 156 int32_t pid_get_value_D(struct pid_filter *p)
 157 {
 158         uint8_t flags;
 159         int32_t ret;
 160         IRQ_LOCK(flags);
 161         ret = p->prev_D;
 162         IRQ_UNLOCK(flags);
 163         return ret;
 164 }
 165
 166 int32_t pid_get_value_out(struct pid_filter *p)
 167 {
 168         uint8_t flags;
 169         int32_t ret;
 170         IRQ_LOCK(flags);
 171         ret = (p->prev_out);
 172         IRQ_UNLOCK(flags);
 173         return ret;
 174 }
 175
 176 /* first parameter should be a (struct pid_filter *) */
 177 int32_t pid_do_filter(void * data, int32_t in)
 178 {
 179         int32_t derivate ;
 180         int32_t command ;
 181         struct pid_filter * p = data;
 182         uint8_t prev_index;
 183
 184         /*
 185          * Integral value : the integral become bigger with time .. (think
 186          * to area of graph, we add one area to the previous) so,
 187          * integral = previous integral + current value
 188          */
 189
 190         /* derivate value
 191         *             f(t+h) - f(t)        with f(t+h) = current value
 192         *  derivate = -------------             f(t)   = previous value
 193         *                    h
 194         * so derivate = current error - previous error
 195         *
 196         * We can apply a filter to reduce noise on the derivate term,
 197         * by using a bigger period.
 198         */
 199
 200         prev_index = p->index + 1;
 201         if (prev_index >= p->derivate_nb_samples)
 202                 prev_index = 0;
 203
 204         /* saturate input... it influences integral an derivate */
 205         if (p->max_in)
 206                 S_MAX(in, p->max_in) ;
 207
 208         derivate = in - p->prev_samples[prev_index];
 209         p->integral += in ;
 210
 211         if (p->max_I)
 212                 S_MAX(p->integral, p->max_I) ;
 213
 214         /* so, command = P.coef_P + I.coef_I + D.coef_D */
 215         command = in * p->gain_P +
 216                 p->integral * p->gain_I +
 217                 (derivate * p->gain_D) / p->derivate_nb_samples ;
 218
 219         if ( command < 0 )
 220                 command = -( -command >> p->out_shift );
 221         else
 222                 command = command >> p->out_shift ;
 223
 224         if (p->max_out)
 225                 S_MAX (command, p->max_out) ;
 226
 227
 228         /* backup of current error value (for the next calcul of derivate value) */
 229         p->prev_samples[p->index] = in ;
 230         p->index = prev_index; /* next index is prev_index */
 231         p->prev_D = derivate ;
 232         p->prev_out = command ;
 233
 234         return command;
 235 }