00001 /* 00002 * Copyright Droids Corporation, Microb Technology, Eirbot (2005) 00003 * 00004 * This program is free software; you can redistribute it and/or modify 00005 * it under the terms of the GNU General Public License as published by 00006 * the Free Software Foundation; either version 2 of the License, or 00007 * (at your option) any later version. 00008 * 00009 * This program is distributed in the hope that it will be useful, 00010 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00011 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00012 * GNU General Public License for more details. 00013 * 00014 * You should have received a copy of the GNU General Public License 00015 * along with this program; if not, write to the Free Software 00016 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00017 * 00018 * 00019 */ 00020 00021 00022 #include <aversive.h> 00023 #include "ramp.h" 00024 00025 00026 /* Initialize the two first fields to the max possible value and previous out to 0*/ 00027 00028 void ramp_init(struct ramp_filter * r) 00029 { 00030 uint8_t flags; 00031 IRQ_LOCK(flags); 00032 00033 r->var_neg=0xFFFFFFFF; 00034 r->var_pos=0xFFFFFFFF; 00035 r->prev_out=0; 00036 00037 IRQ_UNLOCK(flags); 00038 return; 00039 } 00040 00041 /*Set the field var_neg to neg and var_pos to pos */ 00042 00043 void ramp_set_vars(struct ramp_filter * r, uint32_t neg, uint32_t pos) 00044 { 00045 uint8_t flags; 00046 IRQ_LOCK(flags); 00047 00048 r->var_neg=neg; 00049 r->var_pos=pos; 00050 00051 IRQ_UNLOCK(flags); 00052 return; 00053 } 00054 00055 /*Filter the in value using the ramp_filter r*/ 00056 int32_t ramp_do_filter(void * data, int32_t in) 00057 { 00058 uint32_t variation; 00059 struct ramp_filter * r = (struct ramp_filter *) data; 00060 00061 if (in>r->prev_out) /*test if the variation is positive or negative */ 00062 { 00063 variation=in-r->prev_out; /* positive variation */ 00064 if (variation<r->var_pos) /* test if the variation is too high */ 00065 r->prev_out=in; /* variation ok return value will be in */ 00066 else 00067 r->prev_out=r->prev_out+r->var_pos; /* variation too high so return value is filtered */ 00068 } 00069 else 00070 { 00071 variation=r->prev_out-in; /* negative variation */ 00072 if (variation<r->var_neg) /* test if the variation is too high */ 00073 r->prev_out=in; /* variation ok return value will be in */ 00074 else 00075 r->prev_out=r->prev_out-r->var_neg; /* variation too high so return value is filtered */ 00076 } 00077 return(r->prev_out); 00078 } 00079 00080
1.5.6