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.1 2002/09/10 01:14:02 curt
40 Revision 1.3 2000/05/24 04:10:01 curt
41 MSVC5 portability changes contributed by Bruce Finney.
43 Revision 1.2 2000/04/10 18:09:41 curt
44 David Megginson made a few (mostly minor) mods to the LaRCsim files, and
45 it's now possible to choose the LaRCsim model at runtime, as in
51 fgfs --aircraft=uiuc --aircraft-dir=Aircraft-uiuc/Boeing747
53 I did this so that I could play with the UIUC stuff without losing
54 Tony's C172 with its flaps, etc. I did my best to respect the design
55 of the LaRCsim code by staying in C, making only minimal changes, and
56 not introducing any dependencies on the rest of FlightGear. The
57 modified files are attached.
59 Revision 1.1.1.1 1999/06/17 18:07:34 curt
62 Revision 1.1.1.1 1999/04/05 21:32:45 curt
63 Start of 0.6.x branch.
65 Revision 1.5 1998/07/12 03:11:03 curt
66 Removed some printf()'s.
67 Fixed the autopilot integration so it should be able to update it's control
68 positions every time the internal flight model loop is run, and not just
69 once per rendered frame.
70 Added a routine to do the necessary stuff to force an arbitrary altitude
72 Gave the Navion engine just a tad more power.
74 Revision 1.4 1998/01/19 18:40:26 curt
75 Tons of little changes to clean up the code and to remove fatal errors
76 when building with the c++ compiler.
78 Revision 1.3 1998/01/05 22:19:25 curt
79 #ifdef'd out some unused code that was problematic for MSVC++ to compile.
81 Revision 1.2 1997/05/29 22:39:58 curt
82 Working on incorporating the LaRCsim flight model.
84 Revision 1.1 1997/05/29 00:09:57 curt
85 Initial Flight Gear revision.
87 * Revision 1.4 1995/03/15 12:15:23 bjax
88 * Added ls_init_get_set() and ls_init_put_set() and ls_init_init()
91 * Revision 1.3 1994/01/11 19:09:44 bjax
92 * Fixed header includes.
94 * Revision 1.2 1992/12/30 14:04:53 bjax
95 * Added call to ls_step(0, 1).
97 * Revision 1.1 92/12/30 14:02:19 bjax
100 * Revision 1.1 92/12/30 13:21:21 bjax
103 * Revision 1.3 93/12/31 10:34:11 bjax
104 * Added $Log marker as well.
107 ----------------------------------------------------------------------------
111 ----------------------------------------------------------------------------
115 ----------------------------------------------------------------------------
119 ----------------------------------------------------------------------------
123 ----------------------------------------------------------------------------
127 --------------------------------------------------------------------------*/
128 static char rcsid[] = "$Id$";
132 #include "ls_types.h"
136 #include "navion_init.h"
137 #include "ls_model.h"
140 #include "ls_generic.h"
142 #define MAX_NUMBER_OF_CONTINUOUS_STATES 100
143 #define MAX_NUMBER_OF_DISCRETE_STATES 20
145 #define NIL_POINTER 0L
147 #define FACILITY_NAME_STRING "init"
148 #define CURRENT_VERSION 10
150 void cherokee_init( void );
151 void c172_init( void );
166 extern SCALAR Simtime;
168 /* static int Symbols_loaded = 0; */
169 static int Number_of_Continuous_States = 0;
170 static int Number_of_Discrete_States = 0;
171 static cont_state_rec Continuous_States[ MAX_NUMBER_OF_CONTINUOUS_STATES ];
172 static disc_state_rec Discrete_States[ MAX_NUMBER_OF_DISCRETE_STATES ];
175 void ls_init_init( void ) {
179 if (Number_of_Continuous_States == 0)
181 Number_of_Continuous_States = HARDWIRED;
183 for (i=0;i<HARDWIRED;i++)
184 strcpy( Continuous_States[i].Symbol.Mod_Name, "*" );
186 strcpy( Continuous_States[ 0].Symbol.Par_Name,
187 "generic_.geodetic_position_v[0]");
188 strcpy( Continuous_States[ 1].Symbol.Par_Name,
189 "generic_.geodetic_position_v[1]");
190 strcpy( Continuous_States[ 2].Symbol.Par_Name,
191 "generic_.geodetic_position_v[2]");
192 strcpy( Continuous_States[ 3].Symbol.Par_Name,
193 "generic_.v_local_v[0]");
194 strcpy( Continuous_States[ 4].Symbol.Par_Name,
195 "generic_.v_local_v[1]");
196 strcpy( Continuous_States[ 5].Symbol.Par_Name,
197 "generic_.v_local_v[2]");
198 strcpy( Continuous_States[ 6].Symbol.Par_Name,
199 "generic_.euler_angles_v[0]");
200 strcpy( Continuous_States[ 7].Symbol.Par_Name,
201 "generic_.euler_angles_v[1]");
202 strcpy( Continuous_States[ 8].Symbol.Par_Name,
203 "generic_.euler_angles_v[2]");
204 strcpy( Continuous_States[ 9].Symbol.Par_Name,
205 "generic_.omega_body_v[0]");
206 strcpy( Continuous_States[10].Symbol.Par_Name,
207 "generic_.omega_body_v[1]");
208 strcpy( Continuous_States[11].Symbol.Par_Name,
209 "generic_.omega_body_v[2]");
210 strcpy( Continuous_States[12].Symbol.Par_Name,
211 "generic_.earth_position_angle");
214 /* commented out by CLO
215 for (i=0;i<Number_of_Continuous_States;i++)
217 (void) ls_get_sym_val( &Continuous_States[i].Symbol, &error );
218 if (error) Continuous_States[i].Symbol.Addr = NIL_POINTER;
221 for (i=0;i<Number_of_Discrete_States;i++)
223 (void) ls_get_sym_val( &Discrete_States[i].Symbol, &error );
224 if (error) Discrete_States[i].Symbol.Addr = NIL_POINTER;
229 /* not all system have strcasecmp */
231 # define strcasecmp stricmp
234 void ls_init( char * aircraft ) {
239 if (!strcasecmp(aircraft, "c172")) {
240 printf("Initializing LaRCsim for C172\n");
241 current_model = C172;
242 } else if (!strcasecmp(aircraft, "navion")) {
243 printf("Initializing LaRCsim for Navion\n");
244 current_model = NAVION;
245 } else if (!strcasecmp(aircraft, "cherokee")) {
246 printf("Initializing LaRCsim for Cherokee\n");
247 current_model = CHEROKEE;
248 } else if (!strcasecmp(aircraft, "uiuc")) {
249 printf("Initializing LaRCsim for UIUC models\n");
250 current_model = UIUC;
252 printf("Unknown LaRCsim aircraft: %s; defaulting to C172\n", aircraft);
253 current_model = C172;
256 /* printf("LS in init() pos = %.2f\n", Latitude); */
260 /* printf("LS after init_init() pos = %.2f\n", Latitude); */
262 /* move the states to proper values */
264 /* commented out by CLO
265 for(i=0;i<Number_of_Continuous_States;i++)
266 if (Continuous_States[i].Symbol.Addr)
267 ls_set_sym_val( &Continuous_States[i].Symbol,
268 Continuous_States[i].value );
270 for(i=0;i<Number_of_Discrete_States;i++)
271 if (Discrete_States[i].Symbol.Addr)
272 ls_set_sym_val( &Discrete_States[i].Symbol,
273 (double) Discrete_States[i].value );
276 switch (current_model) {
291 /* printf("LS after model_init() pos = %.2f\n", Latitude); */
295 /* printf("LS after ls_step() pos = %.2f\n", Latitude); */
299 #ifdef COMPILE_THIS_CODE_THIS_USELESS_CODE
301 char *ls_init_get_set(char *buffer, char *eob)
302 /* This routine parses the settings file for "init" entries. */
305 static char *fac_name = FACILITY_NAME_STRING;
308 int n, ver, i, error, abrt;
310 enum {cont_states_header, cont_states, disc_states_header, disc_states, done } looking_for;
313 looking_for = cont_states_header;
315 n = sscanf(buffer, "%s", line);
316 if (n == 0) return 0L;
317 if (strncasecmp( fac_name, line, strlen(fac_name) )) return 0L;
319 bufptr = strtok( buffer+strlen(fac_name)+1, "\n");
320 if (bufptr == 0) return 0L;
322 sscanf( bufptr, "%d", &ver );
323 if (ver != CURRENT_VERSION) return 0L;
327 while( !abrt && (eob > bufptr))
329 bufptr = strtok( 0L, "\n");
330 if (bufptr == 0) return 0L;
331 if (strncasecmp( bufptr, "end", 3) == 0) break;
333 sscanf( bufptr, "%s", line );
334 if (line[0] != '#') /* ignore comments */
338 case cont_states_header:
340 if (strncasecmp( line, "continuous_states", 17) == 0)
342 n = sscanf( bufptr, "%s%d", line,
343 &Number_of_Continuous_States );
344 if (n != 2) abrt = 1;
345 looking_for = cont_states;
352 n = sscanf( bufptr, "%s%s%le",
353 Continuous_States[i].Symbol.Mod_Name,
354 Continuous_States[i].Symbol.Par_Name,
355 &Continuous_States[i].value );
356 if (n != 3) abrt = 1;
357 Continuous_States[i].Symbol.Addr = NIL_POINTER;
359 if (i >= Number_of_Continuous_States)
360 looking_for = disc_states_header;
363 case disc_states_header:
365 if (strncasecmp( line, "discrete_states", 15) == 0)
367 n = sscanf( bufptr, "%s%d", line,
368 &Number_of_Discrete_States );
369 if (n != 2) abrt = 1;
370 looking_for = disc_states;
377 n = sscanf( bufptr, "%s%s%ld",
378 Discrete_States[i].Symbol.Mod_Name,
379 Discrete_States[i].Symbol.Par_Name,
380 &Discrete_States[i].value );
381 if (n != 3) abrt = 1;
382 Discrete_States[i].Symbol.Addr = NIL_POINTER;
384 if (i >= Number_of_Discrete_States) looking_for = done;
395 Symbols_loaded = !abrt;
399 #endif /* COMPILE_THIS_CODE_THIS_USELESS_CODE */
402 void ls_init_put_set( FILE *fp )
408 fprintf(fp, "#============================== %s\n", FACILITY_NAME_STRING);
410 fprintf(fp, FACILITY_NAME_STRING);
412 fprintf(fp, "%04d\n", CURRENT_VERSION);
413 fprintf(fp, " continuous_states: %d\n", Number_of_Continuous_States);
414 fprintf(fp, "# module parameter value\n");
415 for (i=0; i<Number_of_Continuous_States; i++)
416 fprintf(fp, " %s\t%s\t%E\n",
417 Continuous_States[i].Symbol.Mod_Name,
418 Continuous_States[i].Symbol.Par_Name,
419 Continuous_States[i].value );
421 fprintf(fp, " discrete_states: %d\n", Number_of_Discrete_States);
422 fprintf(fp, "# module parameter value\n");
423 for (i=0;i<Number_of_Discrete_States;i++)
424 fprintf(fp, " %s\t%s\t%ld\n",
425 Discrete_States[i].Symbol.Mod_Name,
426 Discrete_States[i].Symbol.Par_Name,
427 Discrete_States[i].value );
428 fprintf(fp, "end\n");
432 void ls_save_current_as_ic( void ) {
433 /* Save current states as initial conditions */
437 /* commented out by CLO
438 for(i=0;i<Number_of_Continuous_States;i++)
439 if (Continuous_States[i].Symbol.Addr)
440 Continuous_States[i].value =
441 ls_get_sym_val( &Continuous_States[i].Symbol, &error );
443 for(i=0;i<Number_of_Discrete_States;i++)
444 if (Discrete_States[i].Symbol.Addr)
445 Discrete_States[i].value = (long)
446 ls_get_sym_val( &Discrete_States[i].Symbol, &error );