1 /***************************************************************************
5 ----------------------------------------------------------------------------
7 FUNCTION: Initializes simulation
9 ----------------------------------------------------------------------------
11 MODULE STATUS: incomplete
13 ----------------------------------------------------------------------------
15 GENEALOGY: Written 921230 by Bruce Jackson
17 ----------------------------------------------------------------------------
25 ----------------------------------------------------------------------------
31 950314 Added get_set, put_set, and init routines. EBJ
37 Revision 1.2 2003/07/25 17:53:35 mselig
38 UIUC code initilization mods to tidy things up a bit.
40 Revision 1.1.1.1 2002/09/10 01:14:02 curt
41 Initial revision of FlightGear-0.9.0
43 Revision 1.3 2000/05/24 04:10:01 curt
44 MSVC5 portability changes contributed by Bruce Finney.
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
54 fgfs --aircraft=uiuc --aircraft-dir=Aircraft-uiuc/Boeing747
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.
62 Revision 1.1.1.1 1999/06/17 18:07:34 curt
65 Revision 1.1.1.1 1999/04/05 21:32:45 curt
66 Start of 0.6.x branch.
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
75 Gave the Navion engine just a tad more power.
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.
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.
84 Revision 1.2 1997/05/29 22:39:58 curt
85 Working on incorporating the LaRCsim flight model.
87 Revision 1.1 1997/05/29 00:09:57 curt
88 Initial Flight Gear revision.
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()
94 * Revision 1.3 1994/01/11 19:09:44 bjax
95 * Fixed header includes.
97 * Revision 1.2 1992/12/30 14:04:53 bjax
98 * Added call to ls_step(0, 1).
100 * Revision 1.1 92/12/30 14:02:19 bjax
103 * Revision 1.1 92/12/30 13:21:21 bjax
106 * Revision 1.3 93/12/31 10:34:11 bjax
107 * Added $Log marker as well.
110 ----------------------------------------------------------------------------
114 ----------------------------------------------------------------------------
118 ----------------------------------------------------------------------------
122 ----------------------------------------------------------------------------
126 ----------------------------------------------------------------------------
130 --------------------------------------------------------------------------*/
131 // static char rcsid[] = "$Id$";
135 #include "ls_types.h"
139 #include "navion_init.h"
140 #include "ls_model.h"
143 #include "ls_generic.h"
145 #define MAX_NUMBER_OF_CONTINUOUS_STATES 100
146 #define MAX_NUMBER_OF_DISCRETE_STATES 20
148 #define NIL_POINTER 0L
150 #define FACILITY_NAME_STRING "init"
151 #define CURRENT_VERSION 10
153 void cherokee_init( void );
154 void c172_init( void );
155 void basic_init( void );
170 extern SCALAR Simtime;
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 ];
179 void ls_init_init( void ) {
183 if (Number_of_Continuous_States == 0)
185 Number_of_Continuous_States = HARDWIRED;
187 for (i=0;i<HARDWIRED;i++)
188 strcpy( Continuous_States[i].Symbol.Mod_Name, "*" );
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");
218 /* commented out by CLO
219 for (i=0;i<Number_of_Continuous_States;i++)
221 (void) ls_get_sym_val( &Continuous_States[i].Symbol, &error );
222 if (error) Continuous_States[i].Symbol.Addr = NIL_POINTER;
225 for (i=0;i<Number_of_Discrete_States;i++)
227 (void) ls_get_sym_val( &Discrete_States[i].Symbol, &error );
228 if (error) Discrete_States[i].Symbol.Addr = NIL_POINTER;
233 /* not all system have strcasecmp */
235 # define strcasecmp stricmp
238 void ls_init( char * aircraft ) {
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;
259 printf("Unknown LaRCsim aircraft: %s; defaulting to C172\n", aircraft);
260 current_model = C172;
263 /* printf("LS in init() pos = %.2f\n", Latitude); */
267 /* printf("LS after init_init() pos = %.2f\n", Latitude); */
269 /* move the states to proper values */
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 );
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 );
283 switch (current_model) {
301 /* printf("LS after model_init() pos = %.2f\n", Latitude); */
305 /* printf("LS after ls_step() pos = %.2f\n", Latitude); */
309 #ifdef COMPILE_THIS_CODE_THIS_USELESS_CODE
311 char *ls_init_get_set(char *buffer, char *eob)
312 /* This routine parses the settings file for "init" entries. */
315 static char *fac_name = FACILITY_NAME_STRING;
318 int n, ver, i, error, abrt;
320 enum {cont_states_header, cont_states, disc_states_header, disc_states, done } looking_for;
323 looking_for = cont_states_header;
325 n = sscanf(buffer, "%s", line);
326 if (n == 0) return 0L;
327 if (strncasecmp( fac_name, line, strlen(fac_name) )) return 0L;
329 bufptr = strtok( buffer+strlen(fac_name)+1, "\n");
330 if (bufptr == 0) return 0L;
332 sscanf( bufptr, "%d", &ver );
333 if (ver != CURRENT_VERSION) return 0L;
337 while( !abrt && (eob > bufptr))
339 bufptr = strtok( 0L, "\n");
340 if (bufptr == 0) return 0L;
341 if (strncasecmp( bufptr, "end", 3) == 0) break;
343 sscanf( bufptr, "%s", line );
344 if (line[0] != '#') /* ignore comments */
348 case cont_states_header:
350 if (strncasecmp( line, "continuous_states", 17) == 0)
352 n = sscanf( bufptr, "%s%d", line,
353 &Number_of_Continuous_States );
354 if (n != 2) abrt = 1;
355 looking_for = cont_states;
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;
369 if (i >= Number_of_Continuous_States)
370 looking_for = disc_states_header;
373 case disc_states_header:
375 if (strncasecmp( line, "discrete_states", 15) == 0)
377 n = sscanf( bufptr, "%s%d", line,
378 &Number_of_Discrete_States );
379 if (n != 2) abrt = 1;
380 looking_for = disc_states;
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;
394 if (i >= Number_of_Discrete_States) looking_for = done;
405 Symbols_loaded = !abrt;
409 #endif /* COMPILE_THIS_CODE_THIS_USELESS_CODE */
412 void ls_init_put_set( FILE *fp )
418 fprintf(fp, "#============================== %s\n", FACILITY_NAME_STRING);
420 fprintf(fp, FACILITY_NAME_STRING);
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 );
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");
442 void ls_save_current_as_ic( void ) {
443 /* Save current states as initial conditions */
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 );
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 );