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.3 2000/05/24 04:10:01 curt
38 MSVC5 portability changes contributed by Bruce Finney.
40 Revision 1.2 2000/04/10 18:09:41 curt
41 David Megginson made a few (mostly minor) mods to the LaRCsim files, and
42 it's now possible to choose the LaRCsim model at runtime, as in
48 fgfs --aircraft=uiuc --aircraft-dir=Aircraft-uiuc/Boeing747
50 I did this so that I could play with the UIUC stuff without losing
51 Tony's C172 with its flaps, etc. I did my best to respect the design
52 of the LaRCsim code by staying in C, making only minimal changes, and
53 not introducing any dependencies on the rest of FlightGear. The
54 modified files are attached.
56 Revision 1.1.1.1 1999/06/17 18:07:34 curt
59 Revision 1.1.1.1 1999/04/05 21:32:45 curt
60 Start of 0.6.x branch.
62 Revision 1.5 1998/07/12 03:11:03 curt
63 Removed some printf()'s.
64 Fixed the autopilot integration so it should be able to update it's control
65 positions every time the internal flight model loop is run, and not just
66 once per rendered frame.
67 Added a routine to do the necessary stuff to force an arbitrary altitude
69 Gave the Navion engine just a tad more power.
71 Revision 1.4 1998/01/19 18:40:26 curt
72 Tons of little changes to clean up the code and to remove fatal errors
73 when building with the c++ compiler.
75 Revision 1.3 1998/01/05 22:19:25 curt
76 #ifdef'd out some unused code that was problematic for MSVC++ to compile.
78 Revision 1.2 1997/05/29 22:39:58 curt
79 Working on incorporating the LaRCsim flight model.
81 Revision 1.1 1997/05/29 00:09:57 curt
82 Initial Flight Gear revision.
84 * Revision 1.4 1995/03/15 12:15:23 bjax
85 * Added ls_init_get_set() and ls_init_put_set() and ls_init_init()
88 * Revision 1.3 1994/01/11 19:09:44 bjax
89 * Fixed header includes.
91 * Revision 1.2 1992/12/30 14:04:53 bjax
92 * Added call to ls_step(0, 1).
94 * Revision 1.1 92/12/30 14:02:19 bjax
97 * Revision 1.1 92/12/30 13:21:21 bjax
100 * Revision 1.3 93/12/31 10:34:11 bjax
101 * Added $Log marker as well.
104 ----------------------------------------------------------------------------
108 ----------------------------------------------------------------------------
112 ----------------------------------------------------------------------------
116 ----------------------------------------------------------------------------
120 ----------------------------------------------------------------------------
124 --------------------------------------------------------------------------*/
125 static char rcsid[] = "$Id$";
129 #include "ls_types.h"
133 #include "navion_init.h"
134 #include "ls_model.h"
137 #include "ls_generic.h"
139 #define MAX_NUMBER_OF_CONTINUOUS_STATES 100
140 #define MAX_NUMBER_OF_DISCRETE_STATES 20
142 #define NIL_POINTER 0L
144 #define FACILITY_NAME_STRING "init"
145 #define CURRENT_VERSION 10
147 void cherokee_init( void );
148 void c172_init( void );
163 extern SCALAR Simtime;
165 /* static int Symbols_loaded = 0; */
166 static int Number_of_Continuous_States = 0;
167 static int Number_of_Discrete_States = 0;
168 static cont_state_rec Continuous_States[ MAX_NUMBER_OF_CONTINUOUS_STATES ];
169 static disc_state_rec Discrete_States[ MAX_NUMBER_OF_DISCRETE_STATES ];
172 void ls_init_init( void ) {
176 if (Number_of_Continuous_States == 0)
178 Number_of_Continuous_States = HARDWIRED;
180 for (i=0;i<HARDWIRED;i++)
181 strcpy( Continuous_States[i].Symbol.Mod_Name, "*" );
183 strcpy( Continuous_States[ 0].Symbol.Par_Name,
184 "generic_.geodetic_position_v[0]");
185 strcpy( Continuous_States[ 1].Symbol.Par_Name,
186 "generic_.geodetic_position_v[1]");
187 strcpy( Continuous_States[ 2].Symbol.Par_Name,
188 "generic_.geodetic_position_v[2]");
189 strcpy( Continuous_States[ 3].Symbol.Par_Name,
190 "generic_.v_local_v[0]");
191 strcpy( Continuous_States[ 4].Symbol.Par_Name,
192 "generic_.v_local_v[1]");
193 strcpy( Continuous_States[ 5].Symbol.Par_Name,
194 "generic_.v_local_v[2]");
195 strcpy( Continuous_States[ 6].Symbol.Par_Name,
196 "generic_.euler_angles_v[0]");
197 strcpy( Continuous_States[ 7].Symbol.Par_Name,
198 "generic_.euler_angles_v[1]");
199 strcpy( Continuous_States[ 8].Symbol.Par_Name,
200 "generic_.euler_angles_v[2]");
201 strcpy( Continuous_States[ 9].Symbol.Par_Name,
202 "generic_.omega_body_v[0]");
203 strcpy( Continuous_States[10].Symbol.Par_Name,
204 "generic_.omega_body_v[1]");
205 strcpy( Continuous_States[11].Symbol.Par_Name,
206 "generic_.omega_body_v[2]");
207 strcpy( Continuous_States[12].Symbol.Par_Name,
208 "generic_.earth_position_angle");
211 /* commented out by CLO
212 for (i=0;i<Number_of_Continuous_States;i++)
214 (void) ls_get_sym_val( &Continuous_States[i].Symbol, &error );
215 if (error) Continuous_States[i].Symbol.Addr = NIL_POINTER;
218 for (i=0;i<Number_of_Discrete_States;i++)
220 (void) ls_get_sym_val( &Discrete_States[i].Symbol, &error );
221 if (error) Discrete_States[i].Symbol.Addr = NIL_POINTER;
226 /* not all system have strcasecmp */
228 # define strcasecmp stricmp
231 void ls_init( char * aircraft ) {
236 if (!strcasecmp(aircraft, "c172")) {
237 printf("Initializing LaRCsim for C172\n");
238 current_model = C172;
239 } else if (!strcasecmp(aircraft, "navion")) {
240 printf("Initializing LaRCsim for Navion\n");
241 current_model = NAVION;
242 } else if (!strcasecmp(aircraft, "cherokee")) {
243 printf("Initializing LaRCsim for Cherokee\n");
244 current_model = CHEROKEE;
245 } else if (!strcasecmp(aircraft, "uiuc")) {
246 printf("Initializing LaRCsim for UIUC models\n");
247 current_model = UIUC;
249 printf("Unknown LaRCsim aircraft: %s; defaulting to C172\n", aircraft);
250 current_model = C172;
253 /* printf("LS in init() pos = %.2f\n", Latitude); */
257 /* printf("LS after init_init() pos = %.2f\n", Latitude); */
259 /* move the states to proper values */
261 /* commented out by CLO
262 for(i=0;i<Number_of_Continuous_States;i++)
263 if (Continuous_States[i].Symbol.Addr)
264 ls_set_sym_val( &Continuous_States[i].Symbol,
265 Continuous_States[i].value );
267 for(i=0;i<Number_of_Discrete_States;i++)
268 if (Discrete_States[i].Symbol.Addr)
269 ls_set_sym_val( &Discrete_States[i].Symbol,
270 (double) Discrete_States[i].value );
273 switch (current_model) {
288 /* printf("LS after model_init() pos = %.2f\n", Latitude); */
292 /* printf("LS after ls_step() pos = %.2f\n", Latitude); */
296 #ifdef COMPILE_THIS_CODE_THIS_USELESS_CODE
298 char *ls_init_get_set(char *buffer, char *eob)
299 /* This routine parses the settings file for "init" entries. */
302 static char *fac_name = FACILITY_NAME_STRING;
305 int n, ver, i, error, abrt;
307 enum {cont_states_header, cont_states, disc_states_header, disc_states, done } looking_for;
310 looking_for = cont_states_header;
312 n = sscanf(buffer, "%s", line);
313 if (n == 0) return 0L;
314 if (strncasecmp( fac_name, line, strlen(fac_name) )) return 0L;
316 bufptr = strtok( buffer+strlen(fac_name)+1, "\n");
317 if (bufptr == 0) return 0L;
319 sscanf( bufptr, "%d", &ver );
320 if (ver != CURRENT_VERSION) return 0L;
324 while( !abrt && (eob > bufptr))
326 bufptr = strtok( 0L, "\n");
327 if (bufptr == 0) return 0L;
328 if (strncasecmp( bufptr, "end", 3) == 0) break;
330 sscanf( bufptr, "%s", line );
331 if (line[0] != '#') /* ignore comments */
335 case cont_states_header:
337 if (strncasecmp( line, "continuous_states", 17) == 0)
339 n = sscanf( bufptr, "%s%d", line,
340 &Number_of_Continuous_States );
341 if (n != 2) abrt = 1;
342 looking_for = cont_states;
349 n = sscanf( bufptr, "%s%s%le",
350 Continuous_States[i].Symbol.Mod_Name,
351 Continuous_States[i].Symbol.Par_Name,
352 &Continuous_States[i].value );
353 if (n != 3) abrt = 1;
354 Continuous_States[i].Symbol.Addr = NIL_POINTER;
356 if (i >= Number_of_Continuous_States)
357 looking_for = disc_states_header;
360 case disc_states_header:
362 if (strncasecmp( line, "discrete_states", 15) == 0)
364 n = sscanf( bufptr, "%s%d", line,
365 &Number_of_Discrete_States );
366 if (n != 2) abrt = 1;
367 looking_for = disc_states;
374 n = sscanf( bufptr, "%s%s%ld",
375 Discrete_States[i].Symbol.Mod_Name,
376 Discrete_States[i].Symbol.Par_Name,
377 &Discrete_States[i].value );
378 if (n != 3) abrt = 1;
379 Discrete_States[i].Symbol.Addr = NIL_POINTER;
381 if (i >= Number_of_Discrete_States) looking_for = done;
392 Symbols_loaded = !abrt;
396 #endif /* COMPILE_THIS_CODE_THIS_USELESS_CODE */
399 void ls_init_put_set( FILE *fp )
405 fprintf(fp, "#============================== %s\n", FACILITY_NAME_STRING);
407 fprintf(fp, FACILITY_NAME_STRING);
409 fprintf(fp, "%04d\n", CURRENT_VERSION);
410 fprintf(fp, " continuous_states: %d\n", Number_of_Continuous_States);
411 fprintf(fp, "# module parameter value\n");
412 for (i=0; i<Number_of_Continuous_States; i++)
413 fprintf(fp, " %s\t%s\t%E\n",
414 Continuous_States[i].Symbol.Mod_Name,
415 Continuous_States[i].Symbol.Par_Name,
416 Continuous_States[i].value );
418 fprintf(fp, " discrete_states: %d\n", Number_of_Discrete_States);
419 fprintf(fp, "# module parameter value\n");
420 for (i=0;i<Number_of_Discrete_States;i++)
421 fprintf(fp, " %s\t%s\t%ld\n",
422 Discrete_States[i].Symbol.Mod_Name,
423 Discrete_States[i].Symbol.Par_Name,
424 Discrete_States[i].value );
425 fprintf(fp, "end\n");
429 void ls_save_current_as_ic( void ) {
430 /* Save current states as initial conditions */
434 /* commented out by CLO
435 for(i=0;i<Number_of_Continuous_States;i++)
436 if (Continuous_States[i].Symbol.Addr)
437 Continuous_States[i].value =
438 ls_get_sym_val( &Continuous_States[i].Symbol, &error );
440 for(i=0;i<Number_of_Discrete_States;i++)
441 if (Discrete_States[i].Symbol.Addr)
442 Discrete_States[i].value = (long)
443 ls_get_sym_val( &Discrete_States[i].Symbol, &error );