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