371 lines
9.5 KiB
C
371 lines
9.5 KiB
C
/***************************************************************************
|
|
|
|
TITLE: ls_init.c
|
|
|
|
----------------------------------------------------------------------------
|
|
|
|
FUNCTION: Initializes simulation
|
|
|
|
----------------------------------------------------------------------------
|
|
|
|
MODULE STATUS: incomplete
|
|
|
|
----------------------------------------------------------------------------
|
|
|
|
GENEALOGY: Written 921230 by Bruce Jackson
|
|
|
|
----------------------------------------------------------------------------
|
|
|
|
DESIGNED BY: EBJ
|
|
|
|
CODED BY: EBJ
|
|
|
|
MAINTAINED BY: EBJ
|
|
|
|
----------------------------------------------------------------------------
|
|
|
|
MODIFICATION HISTORY:
|
|
|
|
DATE PURPOSE BY
|
|
|
|
950314 Added get_set, put_set, and init routines. EBJ
|
|
|
|
CURRENT RCS HEADER:
|
|
|
|
$Header$
|
|
$Log$
|
|
Revision 1.4 1998/01/19 18:40:26 curt
|
|
Tons of little changes to clean up the code and to remove fatal errors
|
|
when building with the c++ compiler.
|
|
|
|
Revision 1.3 1998/01/05 22:19:25 curt
|
|
#ifdef'd out some unused code that was problematic for MSVC++ to compile.
|
|
|
|
Revision 1.2 1997/05/29 22:39:58 curt
|
|
Working on incorporating the LaRCsim flight model.
|
|
|
|
Revision 1.1 1997/05/29 00:09:57 curt
|
|
Initial Flight Gear revision.
|
|
|
|
* Revision 1.4 1995/03/15 12:15:23 bjax
|
|
* Added ls_init_get_set() and ls_init_put_set() and ls_init_init()
|
|
* routines. EBJ
|
|
*
|
|
* Revision 1.3 1994/01/11 19:09:44 bjax
|
|
* Fixed header includes.
|
|
*
|
|
* Revision 1.2 1992/12/30 14:04:53 bjax
|
|
* Added call to ls_step(0, 1).
|
|
*
|
|
* Revision 1.1 92/12/30 14:02:19 bjax
|
|
* Initial revision
|
|
*
|
|
* Revision 1.1 92/12/30 13:21:21 bjax
|
|
* Initial revision
|
|
*
|
|
* Revision 1.3 93/12/31 10:34:11 bjax
|
|
* Added $Log marker as well.
|
|
*
|
|
|
|
----------------------------------------------------------------------------
|
|
|
|
REFERENCES:
|
|
|
|
----------------------------------------------------------------------------
|
|
|
|
CALLED BY:
|
|
|
|
----------------------------------------------------------------------------
|
|
|
|
CALLS TO:
|
|
|
|
----------------------------------------------------------------------------
|
|
|
|
INPUTS:
|
|
|
|
----------------------------------------------------------------------------
|
|
|
|
OUTPUTS:
|
|
|
|
--------------------------------------------------------------------------*/
|
|
static char rcsid[] = "$Id$";
|
|
|
|
#include <string.h>
|
|
#include <stdio.h>
|
|
#include "ls_types.h"
|
|
#include "ls_sym.h"
|
|
#include "ls_step.h"
|
|
#include "ls_init.h"
|
|
#include "navion_init.h"
|
|
|
|
/* temp */
|
|
#include "ls_generic.h"
|
|
|
|
#define MAX_NUMBER_OF_CONTINUOUS_STATES 100
|
|
#define MAX_NUMBER_OF_DISCRETE_STATES 20
|
|
#define HARDWIRED 13
|
|
#define NIL_POINTER 0L
|
|
|
|
#define FACILITY_NAME_STRING "init"
|
|
#define CURRENT_VERSION 10
|
|
|
|
typedef struct
|
|
{
|
|
symbol_rec Symbol;
|
|
double value;
|
|
} cont_state_rec;
|
|
|
|
typedef struct
|
|
{
|
|
symbol_rec Symbol;
|
|
long value;
|
|
} disc_state_rec;
|
|
|
|
|
|
extern SCALAR Simtime;
|
|
|
|
/* static int Symbols_loaded = 0; */
|
|
static int Number_of_Continuous_States = 0;
|
|
static int Number_of_Discrete_States = 0;
|
|
static cont_state_rec Continuous_States[ MAX_NUMBER_OF_CONTINUOUS_STATES ];
|
|
static disc_state_rec Discrete_States[ MAX_NUMBER_OF_DISCRETE_STATES ];
|
|
|
|
|
|
void ls_init_init( void ) {
|
|
int i;
|
|
/* int error; */
|
|
|
|
if (Number_of_Continuous_States == 0)
|
|
{
|
|
Number_of_Continuous_States = HARDWIRED;
|
|
|
|
for (i=0;i<HARDWIRED;i++)
|
|
strcpy( Continuous_States[i].Symbol.Mod_Name, "*" );
|
|
|
|
strcpy( Continuous_States[ 0].Symbol.Par_Name,
|
|
"generic_.geodetic_position_v[0]");
|
|
strcpy( Continuous_States[ 1].Symbol.Par_Name,
|
|
"generic_.geodetic_position_v[1]");
|
|
strcpy( Continuous_States[ 2].Symbol.Par_Name,
|
|
"generic_.geodetic_position_v[2]");
|
|
strcpy( Continuous_States[ 3].Symbol.Par_Name,
|
|
"generic_.v_local_v[0]");
|
|
strcpy( Continuous_States[ 4].Symbol.Par_Name,
|
|
"generic_.v_local_v[1]");
|
|
strcpy( Continuous_States[ 5].Symbol.Par_Name,
|
|
"generic_.v_local_v[2]");
|
|
strcpy( Continuous_States[ 6].Symbol.Par_Name,
|
|
"generic_.euler_angles_v[0]");
|
|
strcpy( Continuous_States[ 7].Symbol.Par_Name,
|
|
"generic_.euler_angles_v[1]");
|
|
strcpy( Continuous_States[ 8].Symbol.Par_Name,
|
|
"generic_.euler_angles_v[2]");
|
|
strcpy( Continuous_States[ 9].Symbol.Par_Name,
|
|
"generic_.omega_body_v[0]");
|
|
strcpy( Continuous_States[10].Symbol.Par_Name,
|
|
"generic_.omega_body_v[1]");
|
|
strcpy( Continuous_States[11].Symbol.Par_Name,
|
|
"generic_.omega_body_v[2]");
|
|
strcpy( Continuous_States[12].Symbol.Par_Name,
|
|
"generic_.earth_position_angle");
|
|
}
|
|
|
|
/* commented out by CLO
|
|
for (i=0;i<Number_of_Continuous_States;i++)
|
|
{
|
|
(void) ls_get_sym_val( &Continuous_States[i].Symbol, &error );
|
|
if (error) Continuous_States[i].Symbol.Addr = NIL_POINTER;
|
|
}
|
|
|
|
for (i=0;i<Number_of_Discrete_States;i++)
|
|
{
|
|
(void) ls_get_sym_val( &Discrete_States[i].Symbol, &error );
|
|
if (error) Discrete_States[i].Symbol.Addr = NIL_POINTER;
|
|
}
|
|
*/
|
|
}
|
|
|
|
void ls_init( void ) {
|
|
/* int i; */
|
|
|
|
Simtime = 0;
|
|
|
|
printf("LS in init() pos = %.2f\n", Latitude);
|
|
|
|
ls_init_init();
|
|
|
|
printf("LS after init_init() pos = %.2f\n", Latitude);
|
|
/* move the states to proper values */
|
|
|
|
/* commented out by CLO
|
|
for(i=0;i<Number_of_Continuous_States;i++)
|
|
if (Continuous_States[i].Symbol.Addr)
|
|
ls_set_sym_val( &Continuous_States[i].Symbol,
|
|
Continuous_States[i].value );
|
|
|
|
for(i=0;i<Number_of_Discrete_States;i++)
|
|
if (Discrete_States[i].Symbol.Addr)
|
|
ls_set_sym_val( &Discrete_States[i].Symbol,
|
|
(double) Discrete_States[i].value );
|
|
*/
|
|
|
|
model_init();
|
|
|
|
printf("LS after model_init() pos = %.2f\n", Latitude);
|
|
|
|
ls_step(0.0, -1);
|
|
|
|
printf("LS after ls_step() pos = %.2f\n", Latitude);
|
|
}
|
|
|
|
|
|
#ifdef COMPILE_THIS_CODE_THIS_USELESS_CODE
|
|
|
|
char *ls_init_get_set(char *buffer, char *eob)
|
|
/* This routine parses the settings file for "init" entries. */
|
|
{
|
|
|
|
static char *fac_name = FACILITY_NAME_STRING;
|
|
char *bufptr;
|
|
char line[256];
|
|
int n, ver, i, error, abrt;
|
|
|
|
enum {cont_states_header, cont_states, disc_states_header, disc_states, done } looking_for;
|
|
|
|
abrt = 0;
|
|
looking_for = cont_states_header;
|
|
|
|
n = sscanf(buffer, "%s", line);
|
|
if (n == 0) return 0L;
|
|
if (strncasecmp( fac_name, line, strlen(fac_name) )) return 0L;
|
|
|
|
bufptr = strtok( buffer+strlen(fac_name)+1, "\n");
|
|
if (bufptr == 0) return 0L;
|
|
|
|
sscanf( bufptr, "%d", &ver );
|
|
if (ver != CURRENT_VERSION) return 0L;
|
|
|
|
ls_init_init();
|
|
|
|
while( !abrt && (eob > bufptr))
|
|
{
|
|
bufptr = strtok( 0L, "\n");
|
|
if (bufptr == 0) return 0L;
|
|
if (strncasecmp( bufptr, "end", 3) == 0) break;
|
|
|
|
sscanf( bufptr, "%s", line );
|
|
if (line[0] != '#') /* ignore comments */
|
|
{
|
|
switch (looking_for)
|
|
{
|
|
case cont_states_header:
|
|
{
|
|
if (strncasecmp( line, "continuous_states", 17) == 0)
|
|
{
|
|
n = sscanf( bufptr, "%s%d", line,
|
|
&Number_of_Continuous_States );
|
|
if (n != 2) abrt = 1;
|
|
looking_for = cont_states;
|
|
i = 0;
|
|
}
|
|
break;
|
|
}
|
|
case cont_states:
|
|
{
|
|
n = sscanf( bufptr, "%s%s%le",
|
|
Continuous_States[i].Symbol.Mod_Name,
|
|
Continuous_States[i].Symbol.Par_Name,
|
|
&Continuous_States[i].value );
|
|
if (n != 3) abrt = 1;
|
|
Continuous_States[i].Symbol.Addr = NIL_POINTER;
|
|
i++;
|
|
if (i >= Number_of_Continuous_States)
|
|
looking_for = disc_states_header;
|
|
break;
|
|
}
|
|
case disc_states_header:
|
|
{
|
|
if (strncasecmp( line, "discrete_states", 15) == 0)
|
|
{
|
|
n = sscanf( bufptr, "%s%d", line,
|
|
&Number_of_Discrete_States );
|
|
if (n != 2) abrt = 1;
|
|
looking_for = disc_states;
|
|
i = 0;
|
|
}
|
|
break;
|
|
}
|
|
case disc_states:
|
|
{
|
|
n = sscanf( bufptr, "%s%s%ld",
|
|
Discrete_States[i].Symbol.Mod_Name,
|
|
Discrete_States[i].Symbol.Par_Name,
|
|
&Discrete_States[i].value );
|
|
if (n != 3) abrt = 1;
|
|
Discrete_States[i].Symbol.Addr = NIL_POINTER;
|
|
i++;
|
|
if (i >= Number_of_Discrete_States) looking_for = done;
|
|
}
|
|
case done:
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
Symbols_loaded = !abrt;
|
|
|
|
return bufptr;
|
|
}
|
|
#endif /* COMPILE_THIS_CODE_THIS_USELESS_CODE */
|
|
|
|
|
|
void ls_init_put_set( FILE *fp )
|
|
{
|
|
int i;
|
|
|
|
if (fp==0) return;
|
|
fprintf(fp, "\n");
|
|
fprintf(fp, "#============================== %s\n", FACILITY_NAME_STRING);
|
|
fprintf(fp, "\n");
|
|
fprintf(fp, FACILITY_NAME_STRING);
|
|
fprintf(fp, "\n");
|
|
fprintf(fp, "%04d\n", CURRENT_VERSION);
|
|
fprintf(fp, " continuous_states: %d\n", Number_of_Continuous_States);
|
|
fprintf(fp, "# module parameter value\n");
|
|
for (i=0; i<Number_of_Continuous_States; i++)
|
|
fprintf(fp, " %s\t%s\t%E\n",
|
|
Continuous_States[i].Symbol.Mod_Name,
|
|
Continuous_States[i].Symbol.Par_Name,
|
|
Continuous_States[i].value );
|
|
|
|
fprintf(fp, " discrete_states: %d\n", Number_of_Discrete_States);
|
|
fprintf(fp, "# module parameter value\n");
|
|
for (i=0;i<Number_of_Discrete_States;i++)
|
|
fprintf(fp, " %s\t%s\t%ld\n",
|
|
Discrete_States[i].Symbol.Mod_Name,
|
|
Discrete_States[i].Symbol.Par_Name,
|
|
Discrete_States[i].value );
|
|
fprintf(fp, "end\n");
|
|
return;
|
|
}
|
|
|
|
void ls_save_current_as_ic( void ) {
|
|
/* Save current states as initial conditions */
|
|
|
|
/* int i, error; */
|
|
|
|
/* commented out by CLO
|
|
for(i=0;i<Number_of_Continuous_States;i++)
|
|
if (Continuous_States[i].Symbol.Addr)
|
|
Continuous_States[i].value =
|
|
ls_get_sym_val( &Continuous_States[i].Symbol, &error );
|
|
|
|
for(i=0;i<Number_of_Discrete_States;i++)
|
|
if (Discrete_States[i].Symbol.Addr)
|
|
Discrete_States[i].value = (long)
|
|
ls_get_sym_val( &Discrete_States[i].Symbol, &error );
|
|
*/
|
|
}
|