hostsim test
[aversive.git] / 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 }