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.3  1998/01/05 22:19:25  curt
  38 #ifdef'd out some unused code that was problematic for MSVC++ to compile.
  39
  40 Revision 1.2  1997/05/29 22:39:58  curt
  41 Working on incorporating the LaRCsim flight model.
  42
  43 Revision 1.1  1997/05/29 00:09:57  curt
  44 Initial Flight Gear revision.
  45
  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()
  48  * routines. EBJ
  49  *
  50  * Revision 1.3  1994/01/11  19:09:44  bjax
  51  * Fixed header includes.
  52  *
  53  * Revision 1.2  1992/12/30  14:04:53  bjax
  54  * Added call to ls_step(0, 1).
  55  *
  56  * Revision 1.1  92/12/30  14:02:19  bjax
  57  * Initial revision
  58  *
  59  * Revision 1.1  92/12/30  13:21:21  bjax
  60  * Initial revision
  61  *
  62  * Revision 1.3  93/12/31  10:34:11  bjax
  63  * Added $Log marker as well.
  64  *
  65
  66 ----------------------------------------------------------------------------
  67
  68         REFERENCES:
  69
  70 ----------------------------------------------------------------------------
  71
  72         CALLED BY:
  73
  74 ----------------------------------------------------------------------------
  75
  76         CALLS TO:
  77
  78 ----------------------------------------------------------------------------
  79
  80         INPUTS:
  81
  82 ----------------------------------------------------------------------------
  83
  84         OUTPUTS:
  85
  86 --------------------------------------------------------------------------*/
  87 static char rcsid[] = "$Id$";
  88
  89 #include <string.h>
  90 #include <stdio.h>
  91 #include "ls_types.h"
  92 #include "ls_sym.h"
  93
  94 /* temp */
  95 #include "ls_generic.h"
  96
  97 #define MAX_NUMBER_OF_CONTINUOUS_STATES 100
  98 #define MAX_NUMBER_OF_DISCRETE_STATES  20
  99 #define HARDWIRED 13
 100 #define NIL_POINTER 0L
 101
 102 #define FACILITY_NAME_STRING "init"
 103 #define CURRENT_VERSION 10
 104
 105 typedef struct
 106 {
 107     symbol_rec  Symbol;
 108     double      value;
 109 } cont_state_rec;
 110
 111 typedef struct
 112 {
 113     symbol_rec  Symbol;
 114     long        value;
 115 } disc_state_rec;
 116
 117
 118 extern SCALAR Simtime;
 119
 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 ];
 125
 126
 127 void ls_init_init()
 128 {
 129     int i, error;
 130
 131     if (Number_of_Continuous_States == 0)
 132         {
 133             Number_of_Continuous_States = HARDWIRED;
 134
 135             for (i=0;i<HARDWIRED;i++)
 136                 strcpy( Continuous_States[i].Symbol.Mod_Name, "*" );
 137
 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");
 164         }
 165
 166     /* commented out by CLO
 167     for (i=0;i<Number_of_Continuous_States;i++)
 168         {
 169             (void) ls_get_sym_val( &Continuous_States[i].Symbol, &error );
 170             if (error) Continuous_States[i].Symbol.Addr = NIL_POINTER;
 171         }
 172
 173     for (i=0;i<Number_of_Discrete_States;i++)
 174         {
 175             (void) ls_get_sym_val( &Discrete_States[i].Symbol, &error );
 176             if (error) Discrete_States[i].Symbol.Addr = NIL_POINTER;
 177         }
 178     */
 179 }
 180
 181 void ls_init()
 182 {
 183     int i;
 184
 185     Simtime = 0;
 186
 187     printf("LS in init() pos = %.2f\n", Latitude);
 188
 189     ls_init_init();
 190
 191     printf("LS after init_init() pos = %.2f\n", Latitude);
 192     /* move the states to proper values */
 193
 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 );
 199
 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 );
 204     */
 205
 206     model_init();
 207
 208     printf("LS after model_init() pos = %.2f\n", Latitude);
 209
 210     ls_step(0.0, -1);
 211
 212     printf("LS after ls_step() pos = %.2f\n", Latitude);
 213 }
 214
 215
 216 #ifdef COMPILE_THIS_CODE_THIS_USELESS_CODE
 217
 218 char *ls_init_get_set(char *buffer, char *eob)
 219 /* This routine parses the settings file for "init" entries. */
 220 {
 221
 222     static char *fac_name = FACILITY_NAME_STRING;
 223     char *bufptr;
 224     char line[256];
 225     int n, ver, i, error, abrt;
 226
 227     enum {cont_states_header, cont_states, disc_states_header, disc_states, done } looking_for;
 228
 229     abrt = 0;
 230     looking_for = cont_states_header;
 231
 232     n = sscanf(buffer, "%s", line);
 233     if (n == 0) return 0L;
 234     if (strncasecmp( fac_name, line, strlen(fac_name) )) return 0L;
 235
 236     bufptr = strtok( buffer+strlen(fac_name)+1, "\n");
 237     if (bufptr == 0) return 0L;
 238
 239     sscanf( bufptr, "%d", &ver );
 240     if (ver != CURRENT_VERSION) return 0L;
 241
 242     ls_init_init();
 243
 244     while( !abrt && (eob > bufptr))
 245       {
 246         bufptr = strtok( 0L, "\n");
 247         if (bufptr == 0) return 0L;
 248         if (strncasecmp( bufptr, "end", 3) == 0) break;
 249
 250         sscanf( bufptr, "%s", line );
 251         if (line[0] != '#') /* ignore comments */
 252             {
 253                 switch (looking_for)
 254                     {
 255                     case cont_states_header:
 256                         {
 257                             if (strncasecmp( line, "continuous_states", 17) == 0)
 258                                 {
 259                                     n = sscanf( bufptr, "%s%d", line,
 260                                                 &Number_of_Continuous_States );
 261                                     if (n != 2) abrt = 1;
 262                                     looking_for = cont_states;
 263                                     i = 0;
 264                                 }
 265                             break;
 266                         }
 267                     case cont_states:
 268                         {
 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;
 275                             i++;
 276                             if (i >= Number_of_Continuous_States)
 277                                 looking_for = disc_states_header;
 278                             break;
 279                         }
 280                     case disc_states_header:
 281                         {
 282                             if (strncasecmp( line, "discrete_states", 15) == 0)
 283                                 {
 284                                     n = sscanf( bufptr, "%s%d", line,
 285                                                 &Number_of_Discrete_States );
 286                                     if (n != 2) abrt = 1;
 287                                     looking_for = disc_states;
 288                                     i = 0;
 289                                 }
 290                             break;
 291                         }
 292                     case disc_states:
 293                         {
 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;
 300                             i++;
 301                             if (i >= Number_of_Discrete_States) looking_for = done;
 302                         }
 303                     case done:
 304                         {
 305                             break;
 306                         }
 307                     }
 308
 309             }
 310       }
 311
 312     Symbols_loaded = !abrt;
 313
 314     return bufptr;
 315 }
 316 #endif /* COMPILE_THIS_CODE_THIS_USELESS_CODE */
 317
 318
 319 void ls_init_put_set( FILE *fp )
 320 {
 321     int i;
 322
 323     if (fp==0) return;
 324     fprintf(fp, "\n");
 325     fprintf(fp, "#==============================  %s\n", FACILITY_NAME_STRING);
 326     fprintf(fp, "\n");
 327     fprintf(fp, FACILITY_NAME_STRING);
 328     fprintf(fp, "\n");
 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 );
 337
 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");
 346     return;
 347 }
 348
 349 void ls_save_current_as_ic()
 350 {
 351     /* Save current states as initial conditions */
 352
 353     int i, error;
 354
 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 );
 360
 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 );
 365     */
 366 }