src/FDM/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.1  1999/06/17 18:07:34  curt
  38 Initial revision
  39
  40 Revision 1.1.1.1  1999/04/05 21:32:45  curt
  41 Start of 0.6.x branch.
  42
  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
  49   change.
  50 Gave the Navion engine just a tad more power.
  51
  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.
  55
  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.
  58
  59 Revision 1.2  1997/05/29 22:39:58  curt
  60 Working on incorporating the LaRCsim flight model.
  61
  62 Revision 1.1  1997/05/29 00:09:57  curt
  63 Initial Flight Gear revision.
  64
  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()
  67  * routines. EBJ
  68  *
  69  * Revision 1.3  1994/01/11  19:09:44  bjax
  70  * Fixed header includes.
  71  *
  72  * Revision 1.2  1992/12/30  14:04:53  bjax
  73  * Added call to ls_step(0, 1).
  74  *
  75  * Revision 1.1  92/12/30  14:02:19  bjax
  76  * Initial revision
  77  *
  78  * Revision 1.1  92/12/30  13:21:21  bjax
  79  * Initial revision
  80  *
  81  * Revision 1.3  93/12/31  10:34:11  bjax
  82  * Added $Log marker as well.
  83  *
  84
  85 ----------------------------------------------------------------------------
  86
  87         REFERENCES:
  88
  89 ----------------------------------------------------------------------------
  90
  91         CALLED BY:
  92
  93 ----------------------------------------------------------------------------
  94
  95         CALLS TO:
  96
  97 ----------------------------------------------------------------------------
  98
  99         INPUTS:
 100
 101 ----------------------------------------------------------------------------
 102
 103         OUTPUTS:
 104
 105 --------------------------------------------------------------------------*/
 106 static char rcsid[] = "$Id$";
 107
 108 #include <string.h>
 109 #include <stdio.h>
 110 #include "ls_types.h"
 111 #include "ls_sym.h"
 112 #include "ls_step.h"
 113 #include "ls_init.h"
 114 #include "navion_init.h"
 115
 116 /* temp */
 117 #include "ls_generic.h"
 118
 119 #define MAX_NUMBER_OF_CONTINUOUS_STATES 100
 120 #define MAX_NUMBER_OF_DISCRETE_STATES  20
 121 #define HARDWIRED 13
 122 #define NIL_POINTER 0L
 123
 124 #define FACILITY_NAME_STRING "init"
 125 #define CURRENT_VERSION 10
 126
 127 typedef struct
 128 {
 129     symbol_rec  Symbol;
 130     double      value;
 131 } cont_state_rec;
 132
 133 typedef struct
 134 {
 135     symbol_rec  Symbol;
 136     long        value;
 137 } disc_state_rec;
 138
 139
 140 extern SCALAR Simtime;
 141
 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 ];
 147
 148
 149 void ls_init_init( void ) {
 150     int i;
 151     /* int error; */
 152
 153     if (Number_of_Continuous_States == 0)
 154         {
 155             Number_of_Continuous_States = HARDWIRED;
 156
 157             for (i=0;i<HARDWIRED;i++)
 158                 strcpy( Continuous_States[i].Symbol.Mod_Name, "*" );
 159
 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");
 186         }
 187
 188     /* commented out by CLO
 189     for (i=0;i<Number_of_Continuous_States;i++)
 190         {
 191             (void) ls_get_sym_val( &Continuous_States[i].Symbol, &error );
 192             if (error) Continuous_States[i].Symbol.Addr = NIL_POINTER;
 193         }
 194
 195     for (i=0;i<Number_of_Discrete_States;i++)
 196         {
 197             (void) ls_get_sym_val( &Discrete_States[i].Symbol, &error );
 198             if (error) Discrete_States[i].Symbol.Addr = NIL_POINTER;
 199         }
 200     */
 201 }
 202
 203 void ls_init( void ) {
 204     /* int i; */
 205
 206     Simtime = 0;
 207
 208     /* printf("LS in init() pos = %.2f\n", Latitude); */
 209
 210     ls_init_init();
 211
 212     /* printf("LS after init_init() pos = %.2f\n", Latitude); */
 213
 214     /* move the states to proper values */
 215
 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 );
 221
 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 );
 226     */
 227
 228     model_init();
 229
 230     /* printf("LS after model_init() pos = %.2f\n", Latitude); */
 231
 232     ls_step(0.0, -1);
 233
 234     /* printf("LS after ls_step() pos = %.2f\n", Latitude); */
 235 }
 236
 237
 238 #ifdef COMPILE_THIS_CODE_THIS_USELESS_CODE
 239
 240 char *ls_init_get_set(char *buffer, char *eob)
 241 /* This routine parses the settings file for "init" entries. */
 242 {
 243
 244     static char *fac_name = FACILITY_NAME_STRING;
 245     char *bufptr;
 246     char line[256];
 247     int n, ver, i, error, abrt;
 248
 249     enum {cont_states_header, cont_states, disc_states_header, disc_states, done } looking_for;
 250
 251     abrt = 0;
 252     looking_for = cont_states_header;
 253
 254     n = sscanf(buffer, "%s", line);
 255     if (n == 0) return 0L;
 256     if (strncasecmp( fac_name, line, strlen(fac_name) )) return 0L;
 257
 258     bufptr = strtok( buffer+strlen(fac_name)+1, "\n");
 259     if (bufptr == 0) return 0L;
 260
 261     sscanf( bufptr, "%d", &ver );
 262     if (ver != CURRENT_VERSION) return 0L;
 263
 264     ls_init_init();
 265
 266     while( !abrt && (eob > bufptr))
 267       {
 268         bufptr = strtok( 0L, "\n");
 269         if (bufptr == 0) return 0L;
 270         if (strncasecmp( bufptr, "end", 3) == 0) break;
 271
 272         sscanf( bufptr, "%s", line );
 273         if (line[0] != '#') /* ignore comments */
 274             {
 275                 switch (looking_for)
 276                     {
 277                     case cont_states_header:
 278                         {
 279                             if (strncasecmp( line, "continuous_states", 17) == 0)
 280                                 {
 281                                     n = sscanf( bufptr, "%s%d", line,
 282                                                 &Number_of_Continuous_States );
 283                                     if (n != 2) abrt = 1;
 284                                     looking_for = cont_states;
 285                                     i = 0;
 286                                 }
 287                             break;
 288                         }
 289                     case cont_states:
 290                         {
 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;
 297                             i++;
 298                             if (i >= Number_of_Continuous_States)
 299                                 looking_for = disc_states_header;
 300                             break;
 301                         }
 302                     case disc_states_header:
 303                         {
 304                             if (strncasecmp( line, "discrete_states", 15) == 0)
 305                                 {
 306                                     n = sscanf( bufptr, "%s%d", line,
 307                                                 &Number_of_Discrete_States );
 308                                     if (n != 2) abrt = 1;
 309                                     looking_for = disc_states;
 310                                     i = 0;
 311                                 }
 312                             break;
 313                         }
 314                     case disc_states:
 315                         {
 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;
 322                             i++;
 323                             if (i >= Number_of_Discrete_States) looking_for = done;
 324                         }
 325                     case done:
 326                         {
 327                             break;
 328                         }
 329                     }
 330
 331             }
 332       }
 333
 334     Symbols_loaded = !abrt;
 335
 336     return bufptr;
 337 }
 338 #endif /* COMPILE_THIS_CODE_THIS_USELESS_CODE */
 339
 340
 341 void ls_init_put_set( FILE *fp )
 342 {
 343     int i;
 344
 345     if (fp==0) return;
 346     fprintf(fp, "\n");
 347     fprintf(fp, "#==============================  %s\n", FACILITY_NAME_STRING);
 348     fprintf(fp, "\n");
 349     fprintf(fp, FACILITY_NAME_STRING);
 350     fprintf(fp, "\n");
 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 );
 359
 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");
 368     return;
 369 }
 370
 371 void ls_save_current_as_ic( void ) {
 372     /* Save current states as initial conditions */
 373
 374     /* int i, error; */
 375
 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 );
 381
 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 );
 386     */
 387 }