LaRCsim/ls_init.c

   1 /***************************************************************************
   2
   3         TITLE:  ls_init.c
   4
   5 ----------------------------------------------------------------------------
   6
   7         FUNCTION:       Initializes simulation
   8
   9 ----------------------------------------------------------------------------
  10
  11         MODULE STATUS:  incomplete
  12
  13 ----------------------------------------------------------------------------
  14
  15         GENEALOGY:      Written 921230 by Bruce Jackson
  16
  17 ----------------------------------------------------------------------------
  18
  19         DESIGNED BY:    EBJ
  20
  21         CODED BY:       EBJ
  22
  23         MAINTAINED BY:  EBJ
  24
  25 ----------------------------------------------------------------------------
  26
  27         MODIFICATION HISTORY:
  28
  29         DATE    PURPOSE                                         BY
  30
  31         950314  Added get_set, put_set, and init routines.      EBJ
  32
  33         CURRENT RCS HEADER:
  34
  35 $Header$
  36 $Log$
  37 Revision 1.5  1998/07/12 03:11:03  curt
  38 Removed some printf()'s.
  39 Fixed the autopilot integration so it should be able to update it's control
  40   positions every time the internal flight model loop is run, and not just
  41   once per rendered frame.
  42 Added a routine to do the necessary stuff to force an arbitrary altitude
  43   change.
  44 Gave the Navion engine just a tad more power.
  45
  46 Revision 1.4  1998/01/19 18:40:26  curt
  47 Tons of little changes to clean up the code and to remove fatal errors
  48 when building with the c++ compiler.
  49
  50 Revision 1.3  1998/01/05 22:19:25  curt
  51 #ifdef'd out some unused code that was problematic for MSVC++ to compile.
  52
  53 Revision 1.2  1997/05/29 22:39:58  curt
  54 Working on incorporating the LaRCsim flight model.
  55
  56 Revision 1.1  1997/05/29 00:09:57  curt
  57 Initial Flight Gear revision.
  58
  59  * Revision 1.4  1995/03/15  12:15:23  bjax
  60  * Added ls_init_get_set() and ls_init_put_set() and ls_init_init()
  61  * routines. EBJ
  62  *
  63  * Revision 1.3  1994/01/11  19:09:44  bjax
  64  * Fixed header includes.
  65  *
  66  * Revision 1.2  1992/12/30  14:04:53  bjax
  67  * Added call to ls_step(0, 1).
  68  *
  69  * Revision 1.1  92/12/30  14:02:19  bjax
  70  * Initial revision
  71  *
  72  * Revision 1.1  92/12/30  13:21:21  bjax
  73  * Initial revision
  74  *
  75  * Revision 1.3  93/12/31  10:34:11  bjax
  76  * Added $Log marker as well.
  77  *
  78
  79 ----------------------------------------------------------------------------
  80
  81         REFERENCES:
  82
  83 ----------------------------------------------------------------------------
  84
  85         CALLED BY:
  86
  87 ----------------------------------------------------------------------------
  88
  89         CALLS TO:
  90
  91 ----------------------------------------------------------------------------
  92
  93         INPUTS:
  94
  95 ----------------------------------------------------------------------------
  96
  97         OUTPUTS:
  98
  99 --------------------------------------------------------------------------*/
 100 static char rcsid[] = "$Id$";
 101
 102 #include <string.h>
 103 #include <stdio.h>
 104 #include "ls_types.h"
 105 #include "ls_sym.h"
 106 #include "ls_step.h"
 107 #include "ls_init.h"
 108 #include "navion_init.h"
 109
 110 /* temp */
 111 #include "ls_generic.h"
 112
 113 #define MAX_NUMBER_OF_CONTINUOUS_STATES 100
 114 #define MAX_NUMBER_OF_DISCRETE_STATES  20
 115 #define HARDWIRED 13
 116 #define NIL_POINTER 0L
 117
 118 #define FACILITY_NAME_STRING "init"
 119 #define CURRENT_VERSION 10
 120
 121 typedef struct
 122 {
 123     symbol_rec  Symbol;
 124     double      value;
 125 } cont_state_rec;
 126
 127 typedef struct
 128 {
 129     symbol_rec  Symbol;
 130     long        value;
 131 } disc_state_rec;
 132
 133
 134 extern SCALAR Simtime;
 135
 136 /* static int Symbols_loaded = 0; */
 137 static int Number_of_Continuous_States = 0;
 138 static int Number_of_Discrete_States = 0;
 139 static cont_state_rec   Continuous_States[ MAX_NUMBER_OF_CONTINUOUS_STATES ];
 140 static disc_state_rec   Discrete_States[  MAX_NUMBER_OF_DISCRETE_STATES ];
 141
 142
 143 void ls_init_init( void ) {
 144     int i;
 145     /* int error; */
 146
 147     if (Number_of_Continuous_States == 0)
 148         {
 149             Number_of_Continuous_States = HARDWIRED;
 150
 151             for (i=0;i<HARDWIRED;i++)
 152                 strcpy( Continuous_States[i].Symbol.Mod_Name, "*" );
 153
 154             strcpy( Continuous_States[ 0].Symbol.Par_Name,
 155                     "generic_.geodetic_position_v[0]");
 156             strcpy( Continuous_States[ 1].Symbol.Par_Name,
 157                     "generic_.geodetic_position_v[1]");
 158             strcpy( Continuous_States[ 2].Symbol.Par_Name,
 159                     "generic_.geodetic_position_v[2]");
 160             strcpy( Continuous_States[ 3].Symbol.Par_Name,
 161                     "generic_.v_local_v[0]");
 162             strcpy( Continuous_States[ 4].Symbol.Par_Name,
 163                     "generic_.v_local_v[1]");
 164             strcpy( Continuous_States[ 5].Symbol.Par_Name,
 165                     "generic_.v_local_v[2]");
 166             strcpy( Continuous_States[ 6].Symbol.Par_Name,
 167                     "generic_.euler_angles_v[0]");
 168             strcpy( Continuous_States[ 7].Symbol.Par_Name,
 169                     "generic_.euler_angles_v[1]");
 170             strcpy( Continuous_States[ 8].Symbol.Par_Name,
 171                     "generic_.euler_angles_v[2]");
 172             strcpy( Continuous_States[ 9].Symbol.Par_Name,
 173                     "generic_.omega_body_v[0]");
 174             strcpy( Continuous_States[10].Symbol.Par_Name,
 175                     "generic_.omega_body_v[1]");
 176             strcpy( Continuous_States[11].Symbol.Par_Name,
 177                     "generic_.omega_body_v[2]");
 178             strcpy( Continuous_States[12].Symbol.Par_Name,
 179                     "generic_.earth_position_angle");
 180         }
 181
 182     /* commented out by CLO
 183     for (i=0;i<Number_of_Continuous_States;i++)
 184         {
 185             (void) ls_get_sym_val( &Continuous_States[i].Symbol, &error );
 186             if (error) Continuous_States[i].Symbol.Addr = NIL_POINTER;
 187         }
 188
 189     for (i=0;i<Number_of_Discrete_States;i++)
 190         {
 191             (void) ls_get_sym_val( &Discrete_States[i].Symbol, &error );
 192             if (error) Discrete_States[i].Symbol.Addr = NIL_POINTER;
 193         }
 194     */
 195 }
 196
 197 void ls_init( void ) {
 198     /* int i; */
 199
 200     Simtime = 0;
 201
 202     /* printf("LS in init() pos = %.2f\n", Latitude); */
 203
 204     ls_init_init();
 205
 206     /* printf("LS after init_init() pos = %.2f\n", Latitude); */
 207
 208     /* move the states to proper values */
 209
 210     /* commented out by CLO
 211     for(i=0;i<Number_of_Continuous_States;i++)
 212         if (Continuous_States[i].Symbol.Addr)
 213             ls_set_sym_val( &Continuous_States[i].Symbol,
 214                             Continuous_States[i].value );
 215
 216     for(i=0;i<Number_of_Discrete_States;i++)
 217         if (Discrete_States[i].Symbol.Addr)
 218             ls_set_sym_val( &Discrete_States[i].Symbol,
 219                             (double) Discrete_States[i].value );
 220     */
 221
 222     model_init();
 223
 224     /* printf("LS after model_init() pos = %.2f\n", Latitude); */
 225
 226     ls_step(0.0, -1);
 227
 228     /* printf("LS after ls_step() pos = %.2f\n", Latitude); */
 229 }
 230
 231
 232 #ifdef COMPILE_THIS_CODE_THIS_USELESS_CODE
 233
 234 char *ls_init_get_set(char *buffer, char *eob)
 235 /* This routine parses the settings file for "init" entries. */
 236 {
 237
 238     static char *fac_name = FACILITY_NAME_STRING;
 239     char *bufptr;
 240     char line[256];
 241     int n, ver, i, error, abrt;
 242
 243     enum {cont_states_header, cont_states, disc_states_header, disc_states, done } looking_for;
 244
 245     abrt = 0;
 246     looking_for = cont_states_header;
 247
 248     n = sscanf(buffer, "%s", line);
 249     if (n == 0) return 0L;
 250     if (strncasecmp( fac_name, line, strlen(fac_name) )) return 0L;
 251
 252     bufptr = strtok( buffer+strlen(fac_name)+1, "\n");
 253     if (bufptr == 0) return 0L;
 254
 255     sscanf( bufptr, "%d", &ver );
 256     if (ver != CURRENT_VERSION) return 0L;
 257
 258     ls_init_init();
 259
 260     while( !abrt && (eob > bufptr))
 261       {
 262         bufptr = strtok( 0L, "\n");
 263         if (bufptr == 0) return 0L;
 264         if (strncasecmp( bufptr, "end", 3) == 0) break;
 265
 266         sscanf( bufptr, "%s", line );
 267         if (line[0] != '#') /* ignore comments */
 268             {
 269                 switch (looking_for)
 270                     {
 271                     case cont_states_header:
 272                         {
 273                             if (strncasecmp( line, "continuous_states", 17) == 0)
 274                                 {
 275                                     n = sscanf( bufptr, "%s%d", line,
 276                                                 &Number_of_Continuous_States );
 277                                     if (n != 2) abrt = 1;
 278                                     looking_for = cont_states;
 279                                     i = 0;
 280                                 }
 281                             break;
 282                         }
 283                     case cont_states:
 284                         {
 285                             n = sscanf( bufptr, "%s%s%le",
 286                                         Continuous_States[i].Symbol.Mod_Name,
 287                                         Continuous_States[i].Symbol.Par_Name,
 288                                         &Continuous_States[i].value );
 289                             if (n != 3) abrt = 1;
 290                             Continuous_States[i].Symbol.Addr = NIL_POINTER;
 291                             i++;
 292                             if (i >= Number_of_Continuous_States)
 293                                 looking_for = disc_states_header;
 294                             break;
 295                         }
 296                     case disc_states_header:
 297                         {
 298                             if (strncasecmp( line, "discrete_states", 15) == 0)
 299                                 {
 300                                     n = sscanf( bufptr, "%s%d", line,
 301                                                 &Number_of_Discrete_States );
 302                                     if (n != 2) abrt = 1;
 303                                     looking_for = disc_states;
 304                                     i = 0;
 305                                 }
 306                             break;
 307                         }
 308                     case disc_states:
 309                         {
 310                             n = sscanf( bufptr, "%s%s%ld",
 311                                         Discrete_States[i].Symbol.Mod_Name,
 312                                         Discrete_States[i].Symbol.Par_Name,
 313                                         &Discrete_States[i].value );
 314                             if (n != 3) abrt = 1;
 315                             Discrete_States[i].Symbol.Addr = NIL_POINTER;
 316                             i++;
 317                             if (i >= Number_of_Discrete_States) looking_for = done;
 318                         }
 319                     case done:
 320                         {
 321                             break;
 322                         }
 323                     }
 324
 325             }
 326       }
 327
 328     Symbols_loaded = !abrt;
 329
 330     return bufptr;
 331 }
 332 #endif /* COMPILE_THIS_CODE_THIS_USELESS_CODE */
 333
 334
 335 void ls_init_put_set( FILE *fp )
 336 {
 337     int i;
 338
 339     if (fp==0) return;
 340     fprintf(fp, "\n");
 341     fprintf(fp, "#==============================  %s\n", FACILITY_NAME_STRING);
 342     fprintf(fp, "\n");
 343     fprintf(fp, FACILITY_NAME_STRING);
 344     fprintf(fp, "\n");
 345     fprintf(fp, "%04d\n", CURRENT_VERSION);
 346     fprintf(fp, "  continuous_states: %d\n", Number_of_Continuous_States);
 347     fprintf(fp, "#    module    parameter   value\n");
 348     for (i=0; i<Number_of_Continuous_States; i++)
 349         fprintf(fp, "    %s\t%s\t%E\n",
 350                 Continuous_States[i].Symbol.Mod_Name,
 351                 Continuous_States[i].Symbol.Par_Name,
 352                 Continuous_States[i].value );
 353
 354     fprintf(fp, "  discrete_states: %d\n", Number_of_Discrete_States);
 355     fprintf(fp, "#    module    parameter   value\n");
 356     for (i=0;i<Number_of_Discrete_States;i++)
 357         fprintf(fp, "    %s\t%s\t%ld\n",
 358                 Discrete_States[i].Symbol.Mod_Name,
 359                 Discrete_States[i].Symbol.Par_Name,
 360                 Discrete_States[i].value );
 361     fprintf(fp, "end\n");
 362     return;
 363 }
 364
 365 void ls_save_current_as_ic( void ) {
 366     /* Save current states as initial conditions */
 367
 368     /* int i, error; */
 369
 370     /* commented out by CLO
 371     for(i=0;i<Number_of_Continuous_States;i++)
 372         if (Continuous_States[i].Symbol.Addr)
 373             Continuous_States[i].value =
 374                 ls_get_sym_val( &Continuous_States[i].Symbol, &error );
 375
 376     for(i=0;i<Number_of_Discrete_States;i++)
 377         if (Discrete_States[i].Symbol.Addr)
 378             Discrete_States[i].value = (long)
 379                 ls_get_sym_val( &Discrete_States[i].Symbol, &error );
 380     */
 381 }