1 /***************************************************************************
2
3 TITLE: ls_init.c
4
5 ----------------------------------------------------------------------------
6
7 FUNCTION: Initializes simulation
8
9 ----------------------------------------------------------------------------
10
11 MODULE STATUS: incomplete
12
13 ----------------------------------------------------------------------------
14
15 GENEALOGY: Written 921230 by Bruce Jackson
16
17 ----------------------------------------------------------------------------
18
19 DESIGNED BY: EBJ
20
21 CODED BY: EBJ
22
23 MAINTAINED BY: EBJ
24
25 ----------------------------------------------------------------------------
26
27 MODIFICATION HISTORY:
28
29 DATE PURPOSE BY
30
31 950314 Added get_set, put_set, and init routines. EBJ
32
33 CURRENT RCS HEADER:
34
35 $Header$
36 $Log$
37 Revision 1.2 2003/07/25 17:53:35 mselig
38 UIUC code initilization mods to tidy things up a bit.
39
40 Revision 1.1.1.1 2002/09/10 01:14:02 curt
41 Initial revision of FlightGear-0.9.0
42
43 Revision 1.3 2000/05/24 04:10:01 curt
44 MSVC5 portability changes contributed by Bruce Finney.
45
46 Revision 1.2 2000/04/10 18:09:41 curt
47 David Megginson made a few (mostly minor) mods to the LaRCsim files, and
48 it's now possible to choose the LaRCsim model at runtime, as in
49
50 fgfs --aircraft=c172
51
52 or
53
54 fgfs --aircraft=uiuc --aircraft-dir=Aircraft-uiuc/Boeing747
55
56 I did this so that I could play with the UIUC stuff without losing
57 Tony's C172 with its flaps, etc. I did my best to respect the design
58 of the LaRCsim code by staying in C, making only minimal changes, and
59 not introducing any dependencies on the rest of FlightGear. The
60 modified files are attached.
61
62 Revision 1.1.1.1 1999/06/17 18:07:34 curt
63 Start of 0.7.x branch
64
65 Revision 1.1.1.1 1999/04/05 21:32:45 curt
66 Start of 0.6.x branch.
67
68 Revision 1.5 1998/07/12 03:11:03 curt
69 Removed some printf()'s.
70 Fixed the autopilot integration so it should be able to update it's control
71 positions every time the internal flight model loop is run, and not just
72 once per rendered frame.
73 Added a routine to do the necessary stuff to force an arbitrary altitude
74 change.
75 Gave the Navion engine just a tad more power.
76
77 Revision 1.4 1998/01/19 18:40:26 curt
78 Tons of little changes to clean up the code and to remove fatal errors
79 when building with the c++ compiler.
80
81 Revision 1.3 1998/01/05 22:19:25 curt
82 #ifdef'd out some unused code that was problematic for MSVC++ to compile.
83
84 Revision 1.2 1997/05/29 22:39:58 curt
85 Working on incorporating the LaRCsim flight model.
86
87 Revision 1.1 1997/05/29 00:09:57 curt
88 Initial Flight Gear revision.
89
90 * Revision 1.4 1995/03/15 12:15:23 bjax
91 * Added ls_init_get_set() and ls_init_put_set() and ls_init_init()
92 * routines. EBJ
93 *
94 * Revision 1.3 1994/01/11 19:09:44 bjax
95 * Fixed header includes.
96 *
97 * Revision 1.2 1992/12/30 14:04:53 bjax
98 * Added call to ls_step(0, 1).
99 *
100 * Revision 1.1 92/12/30 14:02:19 bjax
101 * Initial revision
102 *
103 * Revision 1.1 92/12/30 13:21:21 bjax
104 * Initial revision
105 *
106 * Revision 1.3 93/12/31 10:34:11 bjax
107 * Added $Log marker as well.
108 *
109
110 ----------------------------------------------------------------------------
111
112 REFERENCES:
113
114 ----------------------------------------------------------------------------
115
116 CALLED BY:
117
118 ----------------------------------------------------------------------------
119
120 CALLS TO:
121
122 ----------------------------------------------------------------------------
123
124 INPUTS:
125
126 ----------------------------------------------------------------------------
127
128 OUTPUTS:
129
130 --------------------------------------------------------------------------*/
131 // static char rcsid[] = "$Id$";
132
133 #include <string.h>
134 #include <stdio.h>
135 #include "ls_types.h"
136 #include "ls_sym.h"
137 #include "ls_step.h"
138 #include "ls_init.h"
139 #include "navion_init.h"
140 #include "ls_model.h"
141
142 /* temp */
143 #include "ls_generic.h"
144
145 #define MAX_NUMBER_OF_CONTINUOUS_STATES 100
146 #define MAX_NUMBER_OF_DISCRETE_STATES 20
147 #define HARDWIRED 13
148 #define NIL_POINTER 0L
149
150 #define FACILITY_NAME_STRING "init"
151 #define CURRENT_VERSION 10
152
153 void cherokee_init( void );
154 void c172_init( void );
155 void basic_init( void );
156
157 typedef struct
158 {
159 symbol_rec Symbol;
160 double value;
161 } cont_state_rec;
162
163 typedef struct
164 {
165 symbol_rec Symbol;
166 long value;
167 } disc_state_rec;
168
169
170 extern SCALAR Simtime;
171
172 /* static int Symbols_loaded = 0; */
173 static int Number_of_Continuous_States = 0;
174 static int Number_of_Discrete_States = 0;
175 static cont_state_rec Continuous_States[ MAX_NUMBER_OF_CONTINUOUS_STATES ];
176 static disc_state_rec Discrete_States[ MAX_NUMBER_OF_DISCRETE_STATES ];
177
178
ls_init_init(void)179 void ls_init_init( void ) {
180 int i;
181 /* int error; */
182
183 if (Number_of_Continuous_States == 0)
184 {
185 Number_of_Continuous_States = HARDWIRED;
186
187 for (i=0;i<HARDWIRED;i++)
188 strcpy( Continuous_States[i].Symbol.Mod_Name, "*" );
189
190 strcpy( Continuous_States[ 0].Symbol.Par_Name,
191 "generic_.geodetic_position_v[0]");
192 strcpy( Continuous_States[ 1].Symbol.Par_Name,
193 "generic_.geodetic_position_v[1]");
194 strcpy( Continuous_States[ 2].Symbol.Par_Name,
195 "generic_.geodetic_position_v[2]");
196 strcpy( Continuous_States[ 3].Symbol.Par_Name,
197 "generic_.v_local_v[0]");
198 strcpy( Continuous_States[ 4].Symbol.Par_Name,
199 "generic_.v_local_v[1]");
200 strcpy( Continuous_States[ 5].Symbol.Par_Name,
201 "generic_.v_local_v[2]");
202 strcpy( Continuous_States[ 6].Symbol.Par_Name,
203 "generic_.euler_angles_v[0]");
204 strcpy( Continuous_States[ 7].Symbol.Par_Name,
205 "generic_.euler_angles_v[1]");
206 strcpy( Continuous_States[ 8].Symbol.Par_Name,
207 "generic_.euler_angles_v[2]");
208 strcpy( Continuous_States[ 9].Symbol.Par_Name,
209 "generic_.omega_body_v[0]");
210 strcpy( Continuous_States[10].Symbol.Par_Name,
211 "generic_.omega_body_v[1]");
212 strcpy( Continuous_States[11].Symbol.Par_Name,
213 "generic_.omega_body_v[2]");
214 strcpy( Continuous_States[12].Symbol.Par_Name,
215 "generic_.earth_position_angle");
216 }
217
218 /* commented out by CLO
219 for (i=0;i<Number_of_Continuous_States;i++)
220 {
221 (void) ls_get_sym_val( &Continuous_States[i].Symbol, &error );
222 if (error) Continuous_States[i].Symbol.Addr = NIL_POINTER;
223 }
224
225 for (i=0;i<Number_of_Discrete_States;i++)
226 {
227 (void) ls_get_sym_val( &Discrete_States[i].Symbol, &error );
228 if (error) Discrete_States[i].Symbol.Addr = NIL_POINTER;
229 }
230 */
231
232 }
233 /* not all system have strcasecmp */
234 #ifdef _MSC_VER
235 # define strcasecmp stricmp
236 #endif
237
ls_init(char * aircraft)238 void ls_init( char * aircraft ) {
239 /* int i; */
240
241 Simtime = 0;
242
243 if (!strcasecmp(aircraft, "c172")) {
244 printf("Initializing LaRCsim for C172\n");
245 current_model = C172;
246 } else if (!strcasecmp(aircraft, "navion")) {
247 printf("Initializing LaRCsim for Navion\n");
248 current_model = NAVION;
249 } else if (!strcasecmp(aircraft, "cherokee")) {
250 printf("Initializing LaRCsim for Cherokee\n");
251 current_model = CHEROKEE;
252 } else if (!strcasecmp(aircraft, "basic")) {
253 printf("Initializing LaRCsim for Basic\n");
254 current_model = BASIC;
255 } else if (!strcasecmp(aircraft, "uiuc")) {
256 printf("Initializing LaRCsim for UIUC models\n");
257 current_model = UIUC;
258 } else {
259 printf("Unknown LaRCsim aircraft: %s; defaulting to C172\n", aircraft);
260 current_model = C172;
261 }
262
263 /* printf("LS in init() pos = %.2f\n", Latitude); */
264
265 ls_init_init();
266
267 /* printf("LS after init_init() pos = %.2f\n", Latitude); */
268
269 /* move the states to proper values */
270
271 /* commented out by CLO
272 for(i=0;i<Number_of_Continuous_States;i++)
273 if (Continuous_States[i].Symbol.Addr)
274 ls_set_sym_val( &Continuous_States[i].Symbol,
275 Continuous_States[i].value );
276
277 for(i=0;i<Number_of_Discrete_States;i++)
278 if (Discrete_States[i].Symbol.Addr)
279 ls_set_sym_val( &Discrete_States[i].Symbol,
280 (double) Discrete_States[i].value );
281 */
282
283 switch (current_model) {
284 case NAVION:
285 navion_init();
286 break;
287 case C172:
288 c172_init();
289 break;
290 case CHEROKEE:
291 cherokee_init();
292 break;
293 case BASIC:
294 basic_init();
295 break;
296 case UIUC:
297 c172_init();
298 break;
299 }
300
301 /* printf("LS after model_init() pos = %.2f\n", Latitude); */
302
303 ls_step(0.0, -1);
304
305 /* printf("LS after ls_step() pos = %.2f\n", Latitude); */
306 }
307
308
309 #ifdef COMPILE_THIS_CODE_THIS_USELESS_CODE
310
ls_init_get_set(char * buffer,char * eob)311 char *ls_init_get_set(char *buffer, char *eob)
312 /* This routine parses the settings file for "init" entries. */
313 {
314
315 static char *fac_name = FACILITY_NAME_STRING;
316 char *bufptr;
317 char line[256];
318 int n, ver, i, error, abrt;
319
320 enum {cont_states_header, cont_states, disc_states_header, disc_states, done } looking_for;
321
322 abrt = 0;
323 looking_for = cont_states_header;
324
325 n = sscanf(buffer, "%s", line);
326 if (n == 0) return 0L;
327 if (strncasecmp( fac_name, line, strlen(fac_name) )) return 0L;
328
329 bufptr = strtok( buffer+strlen(fac_name)+1, "\n");
330 if (bufptr == 0) return 0L;
331
332 sscanf( bufptr, "%d", &ver );
333 if (ver != CURRENT_VERSION) return 0L;
334
335 ls_init_init();
336
337 while( !abrt && (eob > bufptr))
338 {
339 bufptr = strtok( 0L, "\n");
340 if (bufptr == 0) return 0L;
341 if (strncasecmp( bufptr, "end", 3) == 0) break;
342
343 sscanf( bufptr, "%s", line );
344 if (line[0] != '#') /* ignore comments */
345 {
346 switch (looking_for)
347 {
348 case cont_states_header:
349 {
350 if (strncasecmp( line, "continuous_states", 17) == 0)
351 {
352 n = sscanf( bufptr, "%s%d", line,
353 &Number_of_Continuous_States );
354 if (n != 2) abrt = 1;
355 looking_for = cont_states;
356 i = 0;
357 }
358 break;
359 }
360 case cont_states:
361 {
362 n = sscanf( bufptr, "%s%s%le",
363 Continuous_States[i].Symbol.Mod_Name,
364 Continuous_States[i].Symbol.Par_Name,
365 &Continuous_States[i].value );
366 if (n != 3) abrt = 1;
367 Continuous_States[i].Symbol.Addr = NIL_POINTER;
368 i++;
369 if (i >= Number_of_Continuous_States)
370 looking_for = disc_states_header;
371 break;
372 }
373 case disc_states_header:
374 {
375 if (strncasecmp( line, "discrete_states", 15) == 0)
376 {
377 n = sscanf( bufptr, "%s%d", line,
378 &Number_of_Discrete_States );
379 if (n != 2) abrt = 1;
380 looking_for = disc_states;
381 i = 0;
382 }
383 break;
384 }
385 case disc_states:
386 {
387 n = sscanf( bufptr, "%s%s%ld",
388 Discrete_States[i].Symbol.Mod_Name,
389 Discrete_States[i].Symbol.Par_Name,
390 &Discrete_States[i].value );
391 if (n != 3) abrt = 1;
392 Discrete_States[i].Symbol.Addr = NIL_POINTER;
393 i++;
394 if (i >= Number_of_Discrete_States) looking_for = done;
395 }
396 case done:
397 {
398 break;
399 }
400 }
401
402 }
403 }
404
405 Symbols_loaded = !abrt;
406
407 return bufptr;
408 }
409 #endif /* COMPILE_THIS_CODE_THIS_USELESS_CODE */
410
411
ls_init_put_set(FILE * fp)412 void ls_init_put_set( FILE *fp )
413 {
414 int i;
415
416 if (fp==0) return;
417 fprintf(fp, "\n");
418 fprintf(fp, "#============================== %s\n", FACILITY_NAME_STRING);
419 fprintf(fp, "\n");
420 fprintf(fp, FACILITY_NAME_STRING);
421 fprintf(fp, "\n");
422 fprintf(fp, "%04d\n", CURRENT_VERSION);
423 fprintf(fp, " continuous_states: %d\n", Number_of_Continuous_States);
424 fprintf(fp, "# module parameter value\n");
425 for (i=0; i<Number_of_Continuous_States; i++)
426 fprintf(fp, " %s\t%s\t%E\n",
427 Continuous_States[i].Symbol.Mod_Name,
428 Continuous_States[i].Symbol.Par_Name,
429 Continuous_States[i].value );
430
431 fprintf(fp, " discrete_states: %d\n", Number_of_Discrete_States);
432 fprintf(fp, "# module parameter value\n");
433 for (i=0;i<Number_of_Discrete_States;i++)
434 fprintf(fp, " %s\t%s\t%ld\n",
435 Discrete_States[i].Symbol.Mod_Name,
436 Discrete_States[i].Symbol.Par_Name,
437 Discrete_States[i].value );
438 fprintf(fp, "end\n");
439 return;
440 }
441
ls_save_current_as_ic(void)442 void ls_save_current_as_ic( void ) {
443 /* Save current states as initial conditions */
444
445 /* int i, error; */
446
447 /* commented out by CLO
448 for(i=0;i<Number_of_Continuous_States;i++)
449 if (Continuous_States[i].Symbol.Addr)
450 Continuous_States[i].value =
451 ls_get_sym_val( &Continuous_States[i].Symbol, &error );
452
453 for(i=0;i<Number_of_Discrete_States;i++)
454 if (Discrete_States[i].Symbol.Addr)
455 Discrete_States[i].value = (long)
456 ls_get_sym_val( &Discrete_States[i].Symbol, &error );
457 */
458 }
459