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