modules/devices/control_system/filters/ramp/ramp.c

   1 /*
   2  *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
   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  *
  19  */
  20
  21
  22 #include <aversive.h>
  23 #include "ramp.h"
  24
  25
  26 /* Initialize the two first fields to the max possible value and previous out to 0*/
  27
  28 void ramp_init(struct ramp_filter * r)
  29 {
  30   uint8_t flags;
  31   IRQ_LOCK(flags);
  32
  33   r->var_neg=0xFFFFFFFF;
  34   r->var_pos=0xFFFFFFFF;
  35   r->prev_out=0;
  36
  37   IRQ_UNLOCK(flags);
  38   return;
  39 }
  40
  41 /*Set the field var_neg to neg and var_pos to pos */
  42
  43 void ramp_set_vars(struct ramp_filter * r, uint32_t neg, uint32_t pos)
  44 {
  45   uint8_t flags;
  46   IRQ_LOCK(flags);
  47
  48   r->var_neg=neg;
  49   r->var_pos=pos;
  50
  51   IRQ_UNLOCK(flags);
  52   return;
  53 }
  54
  55 /*Filter the in value using the ramp_filter r*/
  56 int32_t ramp_do_filter(void * data, int32_t in)
  57 {
  58   uint32_t variation;
  59   struct ramp_filter * r = (struct ramp_filter *) data;
  60
  61   if (in>r->prev_out)                           /*test if the variation is positive or negative */
  62     {
  63       variation=in-r->prev_out;                 /* positive variation */
  64       if (variation<r->var_pos)                 /* test if the variation is too high */
  65         r->prev_out=in;                         /* variation ok return value will be in */
  66       else
  67         r->prev_out=r->prev_out+r->var_pos;     /* variation too high so return value is filtered */
  68     }
  69   else
  70     {
  71       variation=r->prev_out-in;                 /* negative variation */
  72       if (variation<r->var_neg)                 /* test if the variation is too high */
  73                 r->prev_out=in;                 /* variation ok return value will be in */
  74       else
  75             r->prev_out=r->prev_out-r->var_neg; /* variation too high so return value is filtered */
  76     }
  77   return(r->prev_out);
  78 }
  79
  80