modules/devices/control_system/control_system_manager/control_system_manager.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  *  Revision : $Id: control_system_manager.c,v 1.7.4.3 2007-12-31 16:25:00 zer0 Exp $
  19  *
  20  */
  21 #include <stdio.h>
  22 #include <control_system_manager.h>
  23
  24 #define DEBUG 0
  25
  26 #if DEBUG == 1
  27 #define debug_printf(args...) do { printf(args); } while(0)
  28 #else
  29 #define debug_printf(args...) do { } while(0)
  30 #endif
  31
  32 /** Call a filter() pointer :
  33  * - lock the interrupts
  34  * - read the pointer to the filter function
  35  * - unlock the interrupts
  36  * - if pointer is null, return the IN value
  37  * - else apply filter
  38  */
  39 static inline uint32_t
  40 safe_filter(int32_t (*f)(void *, int32_t), void * param, int32_t value)
  41 {
  42         int32_t (*f_tmp)(void *, int32_t);
  43         void * param_tmp;
  44         uint8_t flags;
  45         IRQ_LOCK(flags);
  46         f_tmp = f;
  47         param_tmp = param;
  48         IRQ_UNLOCK(flags);
  49         if (f_tmp) {
  50                 return f_tmp(param_tmp, value);
  51         }
  52         return value;
  53 }
  54
  55 /** Call a processout() pointer :
  56  * - lock the interrupts
  57  * - read the pointer to the processout function
  58  * - unlock the interrupts
  59  * - if pointer is null, return 0
  60  * - else return the value processed by the function
  61  */
  62 static inline uint32_t
  63 safe_getprocessout(int32_t (*f)(void *), void * param)
  64 {
  65         int32_t (*f_tmp)(void *);
  66         void * param_tmp;
  67         uint8_t flags;
  68         IRQ_LOCK(flags);
  69         f_tmp = f;
  70         param_tmp = param;
  71         IRQ_UNLOCK(flags);
  72         if (f_tmp) {
  73                 return f_tmp(param_tmp);
  74         }
  75         return 0;
  76 }
  77
  78 /** Call a processin() pointer :
  79  * - lock the interrupts
  80  * - read the pointer to the processin function
  81  * - unlock the interrupts
  82  * - if pointer is null, don't do anything
  83  * - else call the processin with the parameters
  84  */
  85 static inline void
  86 safe_setprocessin(void (*f)(void *, int32_t), void * param, int32_t value)
  87 {
  88         void (*f_tmp)(void *, int32_t);
  89         void * param_tmp;
  90         uint8_t flags;
  91         IRQ_LOCK(flags);
  92         f_tmp = f;
  93         param_tmp = param;
  94         IRQ_UNLOCK(flags);
  95         if (f_tmp) {
  96                 f_tmp(param_tmp, value);
  97         }
  98 }
  99
 100 /**********************************************/
 101
 102 void cs_init(struct cs* cs)
 103 {
 104     uint8_t flags;
 105     IRQ_LOCK(flags);
 106     cs->consign_filter = NULL;
 107     cs->consign_filter_params = NULL;
 108
 109     cs->correct_filter = NULL;
 110     cs->correct_filter_params = NULL;
 111
 112     cs->feedback_filter = NULL;
 113     cs->feedback_filter_params = NULL;
 114
 115     cs->process_out = NULL;
 116     cs->process_out_params = NULL;
 117
 118     cs->process_in = NULL;
 119     cs->process_in_params = NULL;
 120
 121     cs->consign_value = 0;
 122     cs->error_value = 0;
 123     cs->out_value = 0;
 124     IRQ_UNLOCK(flags);
 125
 126     return;
 127 }
 128
 129
 130 void cs_set_consign_filter(struct cs* cs, int32_t (*consign_filter)(void*, int32_t), void* consign_filter_params)
 131 {
 132     uint8_t flags;
 133     IRQ_LOCK(flags);
 134     cs->consign_filter = consign_filter;
 135     cs->consign_filter_params = consign_filter_params;
 136     IRQ_UNLOCK(flags);
 137 }
 138
 139
 140
 141 void cs_set_correct_filter(struct cs* cs, int32_t (*correct_filter)(void*, int32_t), void* correct_filter_params)
 142 {
 143     uint8_t flags;
 144     IRQ_LOCK(flags);
 145     cs->correct_filter = correct_filter;
 146     cs->correct_filter_params = correct_filter_params;
 147     IRQ_UNLOCK(flags);
 148 }
 149
 150
 151 void cs_set_feedback_filter(struct cs* cs, int32_t (*feedback_filter)(void*, int32_t), void* feedback_filter_params)
 152 {
 153     uint8_t flags;
 154     IRQ_LOCK(flags);
 155     cs->feedback_filter = feedback_filter;
 156     cs->feedback_filter_params = feedback_filter_params;
 157     IRQ_UNLOCK(flags);
 158 }
 159
 160
 161 void cs_set_process_in(struct cs* cs, void (*process_in)(void*, int32_t), void* process_in_params)
 162 {
 163     uint8_t flags;
 164     IRQ_LOCK(flags);
 165     cs->process_in = process_in;
 166     cs->process_in_params = process_in_params;
 167     IRQ_UNLOCK(flags);
 168 }
 169
 170
 171
 172 void cs_set_process_out(struct cs* cs, int32_t (*process_out)(void*), void* process_out_params)
 173 {
 174     uint8_t flags;
 175     IRQ_LOCK(flags);
 176     cs->process_out = process_out;
 177     cs->process_out_params = process_out_params;
 178     IRQ_UNLOCK(flags);
 179 }
 180
 181
 182
 183 int32_t cs_do_process(struct cs* cs, int32_t consign)
 184 {
 185 #if DEBUG == 1
 186     static int i=0;
 187 #endif
 188     int32_t process_out_value = 0;
 189
 190     /* save the consign value into the structure */
 191     cs->consign_value = consign;
 192
 193     debug_printf("%d %ld ", i++, consign);
 194
 195     /* if the consign filter exist */
 196     cs->filtered_consign_value = consign = safe_filter(cs->consign_filter, cs->consign_filter_params, consign);
 197
 198     debug_printf("%ld ", cs->filtered_consign_value);
 199
 200     /* read the process out if defined */
 201     process_out_value = safe_getprocessout(cs->process_out, cs->process_out_params);
 202
 203     debug_printf("%ld ", process_out_value);
 204
 205     /* apply the feedback filter if defined */
 206     process_out_value = safe_filter(cs->feedback_filter, cs->feedback_filter_params, process_out_value);
 207     cs->filtered_feedback_value = process_out_value;
 208
 209     debug_printf("%ld ", process_out_value);
 210
 211     /* substract consign and process out and put it into error */
 212     cs->error_value = cs->filtered_consign_value - process_out_value ;
 213
 214     debug_printf("%ld ", cs->error_value);
 215
 216     /* apply the correct filter to error_value and put it into out_value */
 217     cs->out_value = safe_filter(cs->correct_filter, cs->correct_filter_params, cs->error_value);
 218
 219     debug_printf("%ld\n", cs->out_value);
 220
 221     /* send out_value to process in*/
 222     safe_setprocessin (cs->process_in, cs->process_in_params, cs->out_value);
 223
 224     /* return the out value */
 225     return (cs->out_value);
 226 }
 227
 228
 229
 230 void cs_manage(void * data)
 231 {
 232     struct cs* cs = data;
 233     cs_do_process(cs, cs->consign_value);
 234 }
 235
 236
 237
 238 int32_t cs_get_out(struct cs* cs)
 239 {
 240     int32_t tmp;
 241     uint8_t flags;
 242     IRQ_LOCK(flags);
 243     tmp = cs->out_value;
 244     IRQ_UNLOCK(flags);
 245
 246     return tmp;
 247 }
 248
 249
 250
 251 int32_t cs_get_error(struct cs* cs)
 252 {
 253     int32_t tmp;
 254     uint8_t flags;
 255     IRQ_LOCK(flags);
 256     tmp = cs->error_value;
 257     IRQ_UNLOCK(flags);
 258
 259     return tmp;
 260 }
 261
 262
 263
 264 int32_t cs_get_consign(struct cs* cs)
 265 {
 266     int32_t tmp;
 267     uint8_t flags;
 268     IRQ_LOCK(flags);
 269     tmp = cs->consign_value;
 270     IRQ_UNLOCK(flags);
 271
 272     return tmp;
 273 }
 274
 275 int32_t cs_get_filtered_consign(struct cs* cs)
 276 {
 277     int32_t tmp;
 278     uint8_t flags;
 279     IRQ_LOCK(flags);
 280     tmp = cs->filtered_consign_value;
 281     IRQ_UNLOCK(flags);
 282
 283     return tmp;
 284 }
 285
 286 int32_t cs_get_filtered_feedback(struct cs* cs)
 287 {
 288     int32_t tmp;
 289     uint8_t flags;
 290     IRQ_LOCK(flags);
 291     tmp = cs->filtered_feedback_value;
 292     IRQ_UNLOCK(flags);
 293
 294     return tmp;
 295 }
 296
 297
 298
 299 void cs_set_consign(struct cs* cs, int32_t v)
 300 {
 301     uint8_t flags;
 302     /* set the consign */
 303     IRQ_LOCK(flags);
 304     cs->consign_value = v;
 305     IRQ_UNLOCK(flags);
 306 }