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 1998/01/05 22:19:25 curt
38 #ifdef'd out some unused code that was problematic for MSVC++ to compile.
40 Revision 1.2 1997/05/29 22:39:58 curt
41 Working on incorporating the LaRCsim flight model.
43 Revision 1.1 1997/05/29 00:09:57 curt
44 Initial Flight Gear revision.
46 * Revision 1.4 1995/03/15 12:15:23 bjax
47 * Added ls_init_get_set() and ls_init_put_set() and ls_init_init()
50 * Revision 1.3 1994/01/11 19:09:44 bjax
51 * Fixed header includes.
53 * Revision 1.2 1992/12/30 14:04:53 bjax
54 * Added call to ls_step(0, 1).
56 * Revision 1.1 92/12/30 14:02:19 bjax
59 * Revision 1.1 92/12/30 13:21:21 bjax
62 * Revision 1.3 93/12/31 10:34:11 bjax
63 * Added $Log marker as well.
66 ----------------------------------------------------------------------------
70 ----------------------------------------------------------------------------
74 ----------------------------------------------------------------------------
78 ----------------------------------------------------------------------------
82 ----------------------------------------------------------------------------
86 --------------------------------------------------------------------------*/
87 static char rcsid[] = "$Id$";
95 #include "ls_generic.h"
97 #define MAX_NUMBER_OF_CONTINUOUS_STATES 100
98 #define MAX_NUMBER_OF_DISCRETE_STATES 20
100 #define NIL_POINTER 0L
102 #define FACILITY_NAME_STRING "init"
103 #define CURRENT_VERSION 10
118 extern SCALAR Simtime;
120 static int Symbols_loaded = 0;
121 static int Number_of_Continuous_States = 0;
122 static int Number_of_Discrete_States = 0;
123 static cont_state_rec Continuous_States[ MAX_NUMBER_OF_CONTINUOUS_STATES ];
124 static disc_state_rec Discrete_States[ MAX_NUMBER_OF_DISCRETE_STATES ];
131 if (Number_of_Continuous_States == 0)
133 Number_of_Continuous_States = HARDWIRED;
135 for (i=0;i<HARDWIRED;i++)
136 strcpy( Continuous_States[i].Symbol.Mod_Name, "*" );
138 strcpy( Continuous_States[ 0].Symbol.Par_Name,
139 "generic_.geodetic_position_v[0]");
140 strcpy( Continuous_States[ 1].Symbol.Par_Name,
141 "generic_.geodetic_position_v[1]");
142 strcpy( Continuous_States[ 2].Symbol.Par_Name,
143 "generic_.geodetic_position_v[2]");
144 strcpy( Continuous_States[ 3].Symbol.Par_Name,
145 "generic_.v_local_v[0]");
146 strcpy( Continuous_States[ 4].Symbol.Par_Name,
147 "generic_.v_local_v[1]");
148 strcpy( Continuous_States[ 5].Symbol.Par_Name,
149 "generic_.v_local_v[2]");
150 strcpy( Continuous_States[ 6].Symbol.Par_Name,
151 "generic_.euler_angles_v[0]");
152 strcpy( Continuous_States[ 7].Symbol.Par_Name,
153 "generic_.euler_angles_v[1]");
154 strcpy( Continuous_States[ 8].Symbol.Par_Name,
155 "generic_.euler_angles_v[2]");
156 strcpy( Continuous_States[ 9].Symbol.Par_Name,
157 "generic_.omega_body_v[0]");
158 strcpy( Continuous_States[10].Symbol.Par_Name,
159 "generic_.omega_body_v[1]");
160 strcpy( Continuous_States[11].Symbol.Par_Name,
161 "generic_.omega_body_v[2]");
162 strcpy( Continuous_States[12].Symbol.Par_Name,
163 "generic_.earth_position_angle");
166 /* commented out by CLO
167 for (i=0;i<Number_of_Continuous_States;i++)
169 (void) ls_get_sym_val( &Continuous_States[i].Symbol, &error );
170 if (error) Continuous_States[i].Symbol.Addr = NIL_POINTER;
173 for (i=0;i<Number_of_Discrete_States;i++)
175 (void) ls_get_sym_val( &Discrete_States[i].Symbol, &error );
176 if (error) Discrete_States[i].Symbol.Addr = NIL_POINTER;
187 printf("LS in init() pos = %.2f\n", Latitude);
191 printf("LS after init_init() pos = %.2f\n", Latitude);
192 /* move the states to proper values */
194 /* commented out by CLO
195 for(i=0;i<Number_of_Continuous_States;i++)
196 if (Continuous_States[i].Symbol.Addr)
197 ls_set_sym_val( &Continuous_States[i].Symbol,
198 Continuous_States[i].value );
200 for(i=0;i<Number_of_Discrete_States;i++)
201 if (Discrete_States[i].Symbol.Addr)
202 ls_set_sym_val( &Discrete_States[i].Symbol,
203 (double) Discrete_States[i].value );
208 printf("LS after model_init() pos = %.2f\n", Latitude);
212 printf("LS after ls_step() pos = %.2f\n", Latitude);
216 #ifdef COMPILE_THIS_CODE_THIS_USELESS_CODE
218 char *ls_init_get_set(char *buffer, char *eob)
219 /* This routine parses the settings file for "init" entries. */
222 static char *fac_name = FACILITY_NAME_STRING;
225 int n, ver, i, error, abrt;
227 enum {cont_states_header, cont_states, disc_states_header, disc_states, done } looking_for;
230 looking_for = cont_states_header;
232 n = sscanf(buffer, "%s", line);
233 if (n == 0) return 0L;
234 if (strncasecmp( fac_name, line, strlen(fac_name) )) return 0L;
236 bufptr = strtok( buffer+strlen(fac_name)+1, "\n");
237 if (bufptr == 0) return 0L;
239 sscanf( bufptr, "%d", &ver );
240 if (ver != CURRENT_VERSION) return 0L;
244 while( !abrt && (eob > bufptr))
246 bufptr = strtok( 0L, "\n");
247 if (bufptr == 0) return 0L;
248 if (strncasecmp( bufptr, "end", 3) == 0) break;
250 sscanf( bufptr, "%s", line );
251 if (line[0] != '#') /* ignore comments */
255 case cont_states_header:
257 if (strncasecmp( line, "continuous_states", 17) == 0)
259 n = sscanf( bufptr, "%s%d", line,
260 &Number_of_Continuous_States );
261 if (n != 2) abrt = 1;
262 looking_for = cont_states;
269 n = sscanf( bufptr, "%s%s%le",
270 Continuous_States[i].Symbol.Mod_Name,
271 Continuous_States[i].Symbol.Par_Name,
272 &Continuous_States[i].value );
273 if (n != 3) abrt = 1;
274 Continuous_States[i].Symbol.Addr = NIL_POINTER;
276 if (i >= Number_of_Continuous_States)
277 looking_for = disc_states_header;
280 case disc_states_header:
282 if (strncasecmp( line, "discrete_states", 15) == 0)
284 n = sscanf( bufptr, "%s%d", line,
285 &Number_of_Discrete_States );
286 if (n != 2) abrt = 1;
287 looking_for = disc_states;
294 n = sscanf( bufptr, "%s%s%ld",
295 Discrete_States[i].Symbol.Mod_Name,
296 Discrete_States[i].Symbol.Par_Name,
297 &Discrete_States[i].value );
298 if (n != 3) abrt = 1;
299 Discrete_States[i].Symbol.Addr = NIL_POINTER;
301 if (i >= Number_of_Discrete_States) looking_for = done;
312 Symbols_loaded = !abrt;
316 #endif /* COMPILE_THIS_CODE_THIS_USELESS_CODE */
319 void ls_init_put_set( FILE *fp )
325 fprintf(fp, "#============================== %s\n", FACILITY_NAME_STRING);
327 fprintf(fp, FACILITY_NAME_STRING);
329 fprintf(fp, "%04d\n", CURRENT_VERSION);
330 fprintf(fp, " continuous_states: %d\n", Number_of_Continuous_States);
331 fprintf(fp, "# module parameter value\n");
332 for (i=0; i<Number_of_Continuous_States; i++)
333 fprintf(fp, " %s\t%s\t%E\n",
334 Continuous_States[i].Symbol.Mod_Name,
335 Continuous_States[i].Symbol.Par_Name,
336 Continuous_States[i].value );
338 fprintf(fp, " discrete_states: %d\n", Number_of_Discrete_States);
339 fprintf(fp, "# module parameter value\n");
340 for (i=0;i<Number_of_Discrete_States;i++)
341 fprintf(fp, " %s\t%s\t%ld\n",
342 Discrete_States[i].Symbol.Mod_Name,
343 Discrete_States[i].Symbol.Par_Name,
344 Discrete_States[i].value );
345 fprintf(fp, "end\n");
349 void ls_save_current_as_ic()
351 /* Save current states as initial conditions */
355 /* commented out by CLO
356 for(i=0;i<Number_of_Continuous_States;i++)
357 if (Continuous_States[i].Symbol.Addr)
358 Continuous_States[i].value =
359 ls_get_sym_val( &Continuous_States[i].Symbol, &error );
361 for(i=0;i<Number_of_Discrete_States;i++)
362 if (Discrete_States[i].Symbol.Addr)
363 Discrete_States[i].value = (long)
364 ls_get_sym_val( &Discrete_States[i].Symbol, &error );