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 1999/06/17 18:07:34 curt
40 Revision 1.1.1.1 1999/04/05 21:32:45 curt
41 Start of 0.6.x branch.
43 Revision 1.5 1998/07/12 03:11:03 curt
44 Removed some printf()'s.
45 Fixed the autopilot integration so it should be able to update it's control
46 positions every time the internal flight model loop is run, and not just
47 once per rendered frame.
48 Added a routine to do the necessary stuff to force an arbitrary altitude
50 Gave the Navion engine just a tad more power.
52 Revision 1.4 1998/01/19 18:40:26 curt
53 Tons of little changes to clean up the code and to remove fatal errors
54 when building with the c++ compiler.
56 Revision 1.3 1998/01/05 22:19:25 curt
57 #ifdef'd out some unused code that was problematic for MSVC++ to compile.
59 Revision 1.2 1997/05/29 22:39:58 curt
60 Working on incorporating the LaRCsim flight model.
62 Revision 1.1 1997/05/29 00:09:57 curt
63 Initial Flight Gear revision.
65 * Revision 1.4 1995/03/15 12:15:23 bjax
66 * Added ls_init_get_set() and ls_init_put_set() and ls_init_init()
69 * Revision 1.3 1994/01/11 19:09:44 bjax
70 * Fixed header includes.
72 * Revision 1.2 1992/12/30 14:04:53 bjax
73 * Added call to ls_step(0, 1).
75 * Revision 1.1 92/12/30 14:02:19 bjax
78 * Revision 1.1 92/12/30 13:21:21 bjax
81 * Revision 1.3 93/12/31 10:34:11 bjax
82 * Added $Log marker as well.
85 ----------------------------------------------------------------------------
89 ----------------------------------------------------------------------------
93 ----------------------------------------------------------------------------
97 ----------------------------------------------------------------------------
101 ----------------------------------------------------------------------------
105 --------------------------------------------------------------------------*/
106 static char rcsid[] = "$Id$";
110 #include "ls_types.h"
114 #include "navion_init.h"
117 #include "ls_generic.h"
119 #define MAX_NUMBER_OF_CONTINUOUS_STATES 100
120 #define MAX_NUMBER_OF_DISCRETE_STATES 20
122 #define NIL_POINTER 0L
124 #define FACILITY_NAME_STRING "init"
125 #define CURRENT_VERSION 10
140 extern SCALAR Simtime;
142 /* static int Symbols_loaded = 0; */
143 static int Number_of_Continuous_States = 0;
144 static int Number_of_Discrete_States = 0;
145 static cont_state_rec Continuous_States[ MAX_NUMBER_OF_CONTINUOUS_STATES ];
146 static disc_state_rec Discrete_States[ MAX_NUMBER_OF_DISCRETE_STATES ];
149 void ls_init_init( void ) {
153 if (Number_of_Continuous_States == 0)
155 Number_of_Continuous_States = HARDWIRED;
157 for (i=0;i<HARDWIRED;i++)
158 strcpy( Continuous_States[i].Symbol.Mod_Name, "*" );
160 strcpy( Continuous_States[ 0].Symbol.Par_Name,
161 "generic_.geodetic_position_v[0]");
162 strcpy( Continuous_States[ 1].Symbol.Par_Name,
163 "generic_.geodetic_position_v[1]");
164 strcpy( Continuous_States[ 2].Symbol.Par_Name,
165 "generic_.geodetic_position_v[2]");
166 strcpy( Continuous_States[ 3].Symbol.Par_Name,
167 "generic_.v_local_v[0]");
168 strcpy( Continuous_States[ 4].Symbol.Par_Name,
169 "generic_.v_local_v[1]");
170 strcpy( Continuous_States[ 5].Symbol.Par_Name,
171 "generic_.v_local_v[2]");
172 strcpy( Continuous_States[ 6].Symbol.Par_Name,
173 "generic_.euler_angles_v[0]");
174 strcpy( Continuous_States[ 7].Symbol.Par_Name,
175 "generic_.euler_angles_v[1]");
176 strcpy( Continuous_States[ 8].Symbol.Par_Name,
177 "generic_.euler_angles_v[2]");
178 strcpy( Continuous_States[ 9].Symbol.Par_Name,
179 "generic_.omega_body_v[0]");
180 strcpy( Continuous_States[10].Symbol.Par_Name,
181 "generic_.omega_body_v[1]");
182 strcpy( Continuous_States[11].Symbol.Par_Name,
183 "generic_.omega_body_v[2]");
184 strcpy( Continuous_States[12].Symbol.Par_Name,
185 "generic_.earth_position_angle");
188 /* commented out by CLO
189 for (i=0;i<Number_of_Continuous_States;i++)
191 (void) ls_get_sym_val( &Continuous_States[i].Symbol, &error );
192 if (error) Continuous_States[i].Symbol.Addr = NIL_POINTER;
195 for (i=0;i<Number_of_Discrete_States;i++)
197 (void) ls_get_sym_val( &Discrete_States[i].Symbol, &error );
198 if (error) Discrete_States[i].Symbol.Addr = NIL_POINTER;
203 void ls_init( void ) {
208 /* printf("LS in init() pos = %.2f\n", Latitude); */
212 /* printf("LS after init_init() pos = %.2f\n", Latitude); */
214 /* move the states to proper values */
216 /* commented out by CLO
217 for(i=0;i<Number_of_Continuous_States;i++)
218 if (Continuous_States[i].Symbol.Addr)
219 ls_set_sym_val( &Continuous_States[i].Symbol,
220 Continuous_States[i].value );
222 for(i=0;i<Number_of_Discrete_States;i++)
223 if (Discrete_States[i].Symbol.Addr)
224 ls_set_sym_val( &Discrete_States[i].Symbol,
225 (double) Discrete_States[i].value );
230 /* printf("LS after model_init() pos = %.2f\n", Latitude); */
234 /* printf("LS after ls_step() pos = %.2f\n", Latitude); */
238 #ifdef COMPILE_THIS_CODE_THIS_USELESS_CODE
240 char *ls_init_get_set(char *buffer, char *eob)
241 /* This routine parses the settings file for "init" entries. */
244 static char *fac_name = FACILITY_NAME_STRING;
247 int n, ver, i, error, abrt;
249 enum {cont_states_header, cont_states, disc_states_header, disc_states, done } looking_for;
252 looking_for = cont_states_header;
254 n = sscanf(buffer, "%s", line);
255 if (n == 0) return 0L;
256 if (strncasecmp( fac_name, line, strlen(fac_name) )) return 0L;
258 bufptr = strtok( buffer+strlen(fac_name)+1, "\n");
259 if (bufptr == 0) return 0L;
261 sscanf( bufptr, "%d", &ver );
262 if (ver != CURRENT_VERSION) return 0L;
266 while( !abrt && (eob > bufptr))
268 bufptr = strtok( 0L, "\n");
269 if (bufptr == 0) return 0L;
270 if (strncasecmp( bufptr, "end", 3) == 0) break;
272 sscanf( bufptr, "%s", line );
273 if (line[0] != '#') /* ignore comments */
277 case cont_states_header:
279 if (strncasecmp( line, "continuous_states", 17) == 0)
281 n = sscanf( bufptr, "%s%d", line,
282 &Number_of_Continuous_States );
283 if (n != 2) abrt = 1;
284 looking_for = cont_states;
291 n = sscanf( bufptr, "%s%s%le",
292 Continuous_States[i].Symbol.Mod_Name,
293 Continuous_States[i].Symbol.Par_Name,
294 &Continuous_States[i].value );
295 if (n != 3) abrt = 1;
296 Continuous_States[i].Symbol.Addr = NIL_POINTER;
298 if (i >= Number_of_Continuous_States)
299 looking_for = disc_states_header;
302 case disc_states_header:
304 if (strncasecmp( line, "discrete_states", 15) == 0)
306 n = sscanf( bufptr, "%s%d", line,
307 &Number_of_Discrete_States );
308 if (n != 2) abrt = 1;
309 looking_for = disc_states;
316 n = sscanf( bufptr, "%s%s%ld",
317 Discrete_States[i].Symbol.Mod_Name,
318 Discrete_States[i].Symbol.Par_Name,
319 &Discrete_States[i].value );
320 if (n != 3) abrt = 1;
321 Discrete_States[i].Symbol.Addr = NIL_POINTER;
323 if (i >= Number_of_Discrete_States) looking_for = done;
334 Symbols_loaded = !abrt;
338 #endif /* COMPILE_THIS_CODE_THIS_USELESS_CODE */
341 void ls_init_put_set( FILE *fp )
347 fprintf(fp, "#============================== %s\n", FACILITY_NAME_STRING);
349 fprintf(fp, FACILITY_NAME_STRING);
351 fprintf(fp, "%04d\n", CURRENT_VERSION);
352 fprintf(fp, " continuous_states: %d\n", Number_of_Continuous_States);
353 fprintf(fp, "# module parameter value\n");
354 for (i=0; i<Number_of_Continuous_States; i++)
355 fprintf(fp, " %s\t%s\t%E\n",
356 Continuous_States[i].Symbol.Mod_Name,
357 Continuous_States[i].Symbol.Par_Name,
358 Continuous_States[i].value );
360 fprintf(fp, " discrete_states: %d\n", Number_of_Discrete_States);
361 fprintf(fp, "# module parameter value\n");
362 for (i=0;i<Number_of_Discrete_States;i++)
363 fprintf(fp, " %s\t%s\t%ld\n",
364 Discrete_States[i].Symbol.Mod_Name,
365 Discrete_States[i].Symbol.Par_Name,
366 Discrete_States[i].value );
367 fprintf(fp, "end\n");
371 void ls_save_current_as_ic( void ) {
372 /* Save current states as initial conditions */
376 /* commented out by CLO
377 for(i=0;i<Number_of_Continuous_States;i++)
378 if (Continuous_States[i].Symbol.Addr)
379 Continuous_States[i].value =
380 ls_get_sym_val( &Continuous_States[i].Symbol, &error );
382 for(i=0;i<Number_of_Discrete_States;i++)
383 if (Discrete_States[i].Symbol.Addr)
384 Discrete_States[i].value = (long)
385 ls_get_sym_val( &Discrete_States[i].Symbol, &error );