2 * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
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.
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.
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
18 * Revision : $Id: control_system_manager.c,v 1.7.4.3 2007-12-31 16:25:00 zer0 Exp $
22 #include <control_system_manager.h>
27 #define debug_printf(args...) do { printf(args); } while(0)
29 #define debug_printf(args...) do { } while(0)
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
39 static inline uint32_t
40 safe_filter(int32_t (*f)(void *, int32_t), void * param, int32_t value)
42 int32_t (*f_tmp)(void *, int32_t);
50 return f_tmp(param_tmp, value);
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
62 static inline uint32_t
63 safe_getprocessout(int32_t (*f)(void *), void * param)
65 int32_t (*f_tmp)(void *);
73 return f_tmp(param_tmp);
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
86 safe_setprocessin(void (*f)(void *, int32_t), void * param, int32_t value)
88 void (*f_tmp)(void *, int32_t);
96 f_tmp(param_tmp, value);
100 /**********************************************/
102 void cs_init(struct cs* cs)
106 cs->consign_filter = NULL;
107 cs->consign_filter_params = NULL;
109 cs->correct_filter = NULL;
110 cs->correct_filter_params = NULL;
112 cs->feedback_filter = NULL;
113 cs->feedback_filter_params = NULL;
115 cs->process_out = NULL;
116 cs->process_out_params = NULL;
118 cs->process_in = NULL;
119 cs->process_in_params = NULL;
121 cs->consign_value = 0;
130 void cs_set_consign_filter(struct cs* cs, int32_t (*consign_filter)(void*, int32_t), void* consign_filter_params)
134 cs->consign_filter = consign_filter;
135 cs->consign_filter_params = consign_filter_params;
141 void cs_set_correct_filter(struct cs* cs, int32_t (*correct_filter)(void*, int32_t), void* correct_filter_params)
145 cs->correct_filter = correct_filter;
146 cs->correct_filter_params = correct_filter_params;
151 void cs_set_feedback_filter(struct cs* cs, int32_t (*feedback_filter)(void*, int32_t), void* feedback_filter_params)
155 cs->feedback_filter = feedback_filter;
156 cs->feedback_filter_params = feedback_filter_params;
161 void cs_set_process_in(struct cs* cs, void (*process_in)(void*, int32_t), void* process_in_params)
165 cs->process_in = process_in;
166 cs->process_in_params = process_in_params;
172 void cs_set_process_out(struct cs* cs, int32_t (*process_out)(void*), void* process_out_params)
176 cs->process_out = process_out;
177 cs->process_out_params = process_out_params;
183 int32_t cs_do_process(struct cs* cs, int32_t consign)
188 int32_t process_out_value = 0;
190 /* save the consign value into the structure */
191 cs->consign_value = consign;
193 debug_printf("%d %ld ", i++, consign);
195 /* if the consign filter exist */
196 cs->filtered_consign_value = consign = safe_filter(cs->consign_filter, cs->consign_filter_params, consign);
198 debug_printf("%ld ", cs->filtered_consign_value);
200 /* read the process out if defined */
201 process_out_value = safe_getprocessout(cs->process_out, cs->process_out_params);
203 debug_printf("%ld ", process_out_value);
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;
209 debug_printf("%ld ", process_out_value);
211 /* substract consign and process out and put it into error */
212 cs->error_value = cs->filtered_consign_value - process_out_value ;
214 debug_printf("%ld ", cs->error_value);
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);
219 debug_printf("%ld\n", cs->out_value);
221 /* send out_value to process in*/
222 safe_setprocessin (cs->process_in, cs->process_in_params, cs->out_value);
224 /* return the out value */
225 return (cs->out_value);
230 void cs_manage(void * data)
232 struct cs* cs = data;
233 cs_do_process(cs, cs->consign_value);
238 int32_t cs_get_out(struct cs* cs)
251 int32_t cs_get_error(struct cs* cs)
256 tmp = cs->error_value;
264 int32_t cs_get_consign(struct cs* cs)
269 tmp = cs->consign_value;
275 int32_t cs_get_filtered_consign(struct cs* cs)
280 tmp = cs->filtered_consign_value;
286 int32_t cs_get_filtered_feedback(struct cs* cs)
291 tmp = cs->filtered_feedback_value;
299 void cs_set_consign(struct cs* cs, int32_t v)
302 /* set the consign */
304 cs->consign_value = v;