FDM/LaRCsim.cxx

   1 // LaRCsim.cxx -- interface to the LaRCsim flight model
   2 //
   3 // Written by Curtis Olson, started October 1998.
   4 //
   5 // Copyright (C) 1998  Curtis L. Olson  - curt@me.umn.edu
   6 //
   7 // This program is free software; you can redistribute it and/or
   8 // modify it under the terms of the GNU General Public License as
   9 // published by the Free Software Foundation; either version 2 of the
  10 // License, or (at your option) any later version.
  11 //
  12 // This program is distributed in the hope that it will be useful, but
  13 // WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15 // General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU General Public License
  18 // along with this program; if not, write to the Free Software
  19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  20 //
  21 // $Id$
  22 // (Log is kept at end of this file)
  23
  24
  25 #include "LaRCsim.hxx"
  26
  27 #include <Aircraft/aircraft.hxx>
  28 #include <Controls/controls.hxx>
  29 #include <Flight/flight.hxx>
  30 #include <Flight/LaRCsim/ls_cockpit.h>
  31 #include <Flight/LaRCsim/ls_generic.h>
  32 #include <Flight/LaRCsim/ls_interface.h>
  33
  34
  35 // Initialize the LaRCsim flight model, dt is the time increment for
  36 // each subsequent iteration through the EOM
  37 int fgLaRCsimInit(double dt) {
  38     ls_toplevel_init(dt);
  39
  40     return(1);
  41 }
  42
  43
  44 // Run an iteration of the EOM (equations of motion)
  45 int fgLaRCsimUpdate(fgFLIGHT *f, int multiloop) {
  46     double save_alt = 0.0;
  47
  48     // lets try to avoid really screwing up the LaRCsim model
  49     if ( FG_Altitude < -9000 ) {
  50         save_alt = FG_Altitude;
  51         FG_Altitude = 0;
  52     }
  53
  54     // translate FG to LaRCsim structure
  55     fgFlight_2_LaRCsim(f);
  56     // printf("FG_Altitude = %.2f\n", FG_Altitude * 0.3048);
  57     // printf("Altitude = %.2f\n", Altitude * 0.3048);
  58     // printf("Radius to Vehicle = %.2f\n", Radius_to_vehicle * 0.3048);
  59
  60     ls_update(multiloop);
  61
  62     // printf("%d FG_Altitude = %.2f\n", i, FG_Altitude * 0.3048);
  63     // printf("%d Altitude = %.2f\n", i, Altitude * 0.3048);
  64
  65     // translate LaRCsim back to FG structure so that the
  66     // autopilot (and the rest of the sim can use the updated
  67     // values
  68     fgLaRCsim_2_Flight(f);
  69
  70     // but lets restore our original bogus altitude when we are done
  71     if ( save_alt < -9000 ) {
  72         FG_Altitude = save_alt;
  73     }
  74
  75     return 1;
  76 }
  77
  78
  79 // Convert from the fgFLIGHT struct to the LaRCsim generic_ struct
  80 int fgFlight_2_LaRCsim (fgFLIGHT *f) {
  81     fgCONTROLS *c;
  82
  83     c = current_aircraft.controls;
  84
  85     Lat_control = FG_Aileron;
  86     Long_control = FG_Elevator;
  87     Long_trim = FG_Elev_Trim;
  88     Rudder_pedal = FG_Rudder;
  89     Throttle_pct = FG_Throttle[0];
  90     Brake_pct = FG_Brake_Amt;
  91
  92     Mass =      FG_Mass;
  93     I_xx =      FG_I_xx;
  94     I_yy =      FG_I_yy;
  95     I_zz =      FG_I_zz;
  96     I_xz =      FG_I_xz;
  97     Dx_pilot =  FG_Dx_pilot;
  98     Dy_pilot =  FG_Dy_pilot;
  99     Dz_pilot =  FG_Dz_pilot;
 100     Dx_cg =     FG_Dx_cg;
 101     Dy_cg =     FG_Dy_cg;
 102     Dz_cg =     FG_Dz_cg;
 103     F_X =       FG_F_X;
 104     F_Y =       FG_F_Y;
 105     F_Z =       FG_F_Z;
 106     F_north =   FG_F_north;
 107     F_east =    FG_F_east;
 108     F_down =    FG_F_down;
 109     F_X_aero =  FG_F_X_aero;
 110     F_Y_aero =  FG_F_Y_aero;
 111     F_Z_aero =  FG_F_Z_aero;
 112     F_X_engine =        FG_F_X_engine;
 113     F_Y_engine =        FG_F_Y_engine;
 114     F_Z_engine =        FG_F_Z_engine;
 115     F_X_gear =  FG_F_X_gear;
 116     F_Y_gear =  FG_F_Y_gear;
 117     F_Z_gear =  FG_F_Z_gear;
 118     M_l_rp =    FG_M_l_rp;
 119     M_m_rp =    FG_M_m_rp;
 120     M_n_rp =    FG_M_n_rp;
 121     M_l_cg =    FG_M_l_cg;
 122     M_m_cg =    FG_M_m_cg;
 123     M_n_cg =    FG_M_n_cg;
 124     M_l_aero =  FG_M_l_aero;
 125     M_m_aero =  FG_M_m_aero;
 126     M_n_aero =  FG_M_n_aero;
 127     M_l_engine =        FG_M_l_engine;
 128     M_m_engine =        FG_M_m_engine;
 129     M_n_engine =        FG_M_n_engine;
 130     M_l_gear =  FG_M_l_gear;
 131     M_m_gear =  FG_M_m_gear;
 132     M_n_gear =  FG_M_n_gear;
 133     V_dot_north =       FG_V_dot_north;
 134     V_dot_east =        FG_V_dot_east;
 135     V_dot_down =        FG_V_dot_down;
 136     U_dot_body =        FG_U_dot_body;
 137     V_dot_body =        FG_V_dot_body;
 138     W_dot_body =        FG_W_dot_body;
 139     A_X_cg =    FG_A_X_cg;
 140     A_Y_cg =    FG_A_Y_cg;
 141     A_Z_cg =    FG_A_Z_cg;
 142     A_X_pilot = FG_A_X_pilot;
 143     A_Y_pilot = FG_A_Y_pilot;
 144     A_Z_pilot = FG_A_Z_pilot;
 145     N_X_cg =    FG_N_X_cg;
 146     N_Y_cg =    FG_N_Y_cg;
 147     N_Z_cg =    FG_N_Z_cg;
 148     N_X_pilot = FG_N_X_pilot;
 149     N_Y_pilot = FG_N_Y_pilot;
 150     N_Z_pilot = FG_N_Z_pilot;
 151     P_dot_body =        FG_P_dot_body;
 152     Q_dot_body =        FG_Q_dot_body;
 153     R_dot_body =        FG_R_dot_body;
 154     V_north =   FG_V_north;
 155     V_east =    FG_V_east;
 156     V_down =    FG_V_down;
 157     V_north_rel_ground =        FG_V_north_rel_ground;
 158     V_east_rel_ground = FG_V_east_rel_ground;
 159     V_down_rel_ground = FG_V_down_rel_ground;
 160     V_north_airmass =   FG_V_north_airmass;
 161     V_east_airmass =    FG_V_east_airmass;
 162     V_down_airmass =    FG_V_down_airmass;
 163     V_north_rel_airmass =       FG_V_north_rel_airmass;
 164     V_east_rel_airmass =        FG_V_east_rel_airmass;
 165     V_down_rel_airmass =        FG_V_down_rel_airmass;
 166     U_gust =    FG_U_gust;
 167     V_gust =    FG_V_gust;
 168     W_gust =    FG_W_gust;
 169     U_body =    FG_U_body;
 170     V_body =    FG_V_body;
 171     W_body =    FG_W_body;
 172     V_rel_wind =        FG_V_rel_wind;
 173     V_true_kts =        FG_V_true_kts;
 174     V_rel_ground =      FG_V_rel_ground;
 175     V_inertial =        FG_V_inertial;
 176     V_ground_speed =    FG_V_ground_speed;
 177     V_equiv =   FG_V_equiv;
 178     V_equiv_kts =       FG_V_equiv_kts;
 179     V_calibrated =      FG_V_calibrated;
 180     V_calibrated_kts =  FG_V_calibrated_kts;
 181     P_body =    FG_P_body;
 182     Q_body =    FG_Q_body;
 183     R_body =    FG_R_body;
 184     P_local =   FG_P_local;
 185     Q_local =   FG_Q_local;
 186     R_local =   FG_R_local;
 187     P_total =   FG_P_total;
 188     Q_total =   FG_Q_total;
 189     R_total =   FG_R_total;
 190     Phi_dot =   FG_Phi_dot;
 191     Theta_dot = FG_Theta_dot;
 192     Psi_dot =   FG_Psi_dot;
 193     Latitude_dot =      FG_Latitude_dot;
 194     Longitude_dot =     FG_Longitude_dot;
 195     Radius_dot =        FG_Radius_dot;
 196     Lat_geocentric =    FG_Lat_geocentric;
 197     Lon_geocentric =    FG_Lon_geocentric;
 198     Radius_to_vehicle = FG_Radius_to_vehicle;
 199     Latitude =  FG_Latitude;
 200     Longitude = FG_Longitude;
 201     Altitude =  FG_Altitude;
 202     Phi =       FG_Phi;
 203     Theta =     FG_Theta;
 204     Psi =       FG_Psi;
 205     T_local_to_body_11 =        FG_T_local_to_body_11;
 206     T_local_to_body_12 =        FG_T_local_to_body_12;
 207     T_local_to_body_13 =        FG_T_local_to_body_13;
 208     T_local_to_body_21 =        FG_T_local_to_body_21;
 209     T_local_to_body_22 =        FG_T_local_to_body_22;
 210     T_local_to_body_23 =        FG_T_local_to_body_23;
 211     T_local_to_body_31 =        FG_T_local_to_body_31;
 212     T_local_to_body_32 =        FG_T_local_to_body_32;
 213     T_local_to_body_33 =        FG_T_local_to_body_33;
 214     Gravity =   FG_Gravity;
 215     Centrifugal_relief =        FG_Centrifugal_relief;
 216     Alpha =     FG_Alpha;
 217     Beta =      FG_Beta;
 218     Alpha_dot = FG_Alpha_dot;
 219     Beta_dot =  FG_Beta_dot;
 220     Cos_alpha = FG_Cos_alpha;
 221     Sin_alpha = FG_Sin_alpha;
 222     Cos_beta =  FG_Cos_beta;
 223     Sin_beta =  FG_Sin_beta;
 224     Cos_phi =   FG_Cos_phi;
 225     Sin_phi =   FG_Sin_phi;
 226     Cos_theta = FG_Cos_theta;
 227     Sin_theta = FG_Sin_theta;
 228     Cos_psi =   FG_Cos_psi;
 229     Sin_psi =   FG_Sin_psi;
 230     Gamma_vert_rad =    FG_Gamma_vert_rad;
 231     Gamma_horiz_rad =   FG_Gamma_horiz_rad;
 232     Sigma =     FG_Sigma;
 233     Density =   FG_Density;
 234     V_sound =   FG_V_sound;
 235     Mach_number =       FG_Mach_number;
 236     Static_pressure =   FG_Static_pressure;
 237     Total_pressure =    FG_Total_pressure;
 238     Impact_pressure =   FG_Impact_pressure;
 239     Dynamic_pressure =  FG_Dynamic_pressure;
 240     Static_temperature =        FG_Static_temperature;
 241     Total_temperature = FG_Total_temperature;
 242     Sea_level_radius =  FG_Sea_level_radius;
 243     Earth_position_angle =      FG_Earth_position_angle;
 244     Runway_altitude =   FG_Runway_altitude;
 245     Runway_latitude =   FG_Runway_latitude;
 246     Runway_longitude =  FG_Runway_longitude;
 247     Runway_heading =    FG_Runway_heading;
 248     Radius_to_rwy =     FG_Radius_to_rwy;
 249     D_cg_north_of_rwy = FG_D_cg_north_of_rwy;
 250     D_cg_east_of_rwy =  FG_D_cg_east_of_rwy;
 251     D_cg_above_rwy =    FG_D_cg_above_rwy;
 252     X_cg_rwy =  FG_X_cg_rwy;
 253     Y_cg_rwy =  FG_Y_cg_rwy;
 254     H_cg_rwy =  FG_H_cg_rwy;
 255     D_pilot_north_of_rwy =      FG_D_pilot_north_of_rwy;
 256     D_pilot_east_of_rwy =       FG_D_pilot_east_of_rwy;
 257     D_pilot_above_rwy = FG_D_pilot_above_rwy;
 258     X_pilot_rwy =       FG_X_pilot_rwy;
 259     Y_pilot_rwy =       FG_Y_pilot_rwy;
 260     H_pilot_rwy =       FG_H_pilot_rwy;
 261
 262     return( 0 );
 263 }
 264
 265
 266 // Convert from the LaRCsim generic_ struct to the fgFLIGHT struct
 267 int fgLaRCsim_2_Flight (fgFLIGHT *f) {
 268     FG_Mass =   Mass;
 269     FG_I_xx =   I_xx;
 270     FG_I_yy =   I_yy;
 271     FG_I_zz =   I_zz;
 272     FG_I_xz =   I_xz;
 273     FG_Dx_pilot =       Dx_pilot;
 274     FG_Dy_pilot =       Dy_pilot;
 275     FG_Dz_pilot =       Dz_pilot;
 276     FG_Dx_cg =  Dx_cg;
 277     FG_Dy_cg =  Dy_cg;
 278     FG_Dz_cg =  Dz_cg;
 279     FG_F_X =    F_X;
 280     FG_F_Y =    F_Y;
 281     FG_F_Z =    F_Z;
 282     FG_F_north =        F_north;
 283     FG_F_east = F_east;
 284     FG_F_down = F_down;
 285     FG_F_X_aero =       F_X_aero;
 286     FG_F_Y_aero =       F_Y_aero;
 287     FG_F_Z_aero =       F_Z_aero;
 288     FG_F_X_engine =     F_X_engine;
 289     FG_F_Y_engine =     F_Y_engine;
 290     FG_F_Z_engine =     F_Z_engine;
 291     FG_F_X_gear =       F_X_gear;
 292     FG_F_Y_gear =       F_Y_gear;
 293     FG_F_Z_gear =       F_Z_gear;
 294     FG_M_l_rp = M_l_rp;
 295     FG_M_m_rp = M_m_rp;
 296     FG_M_n_rp = M_n_rp;
 297     FG_M_l_cg = M_l_cg;
 298     FG_M_m_cg = M_m_cg;
 299     FG_M_n_cg = M_n_cg;
 300     FG_M_l_aero =       M_l_aero;
 301     FG_M_m_aero =       M_m_aero;
 302     FG_M_n_aero =       M_n_aero;
 303     FG_M_l_engine =     M_l_engine;
 304     FG_M_m_engine =     M_m_engine;
 305     FG_M_n_engine =     M_n_engine;
 306     FG_M_l_gear =       M_l_gear;
 307     FG_M_m_gear =       M_m_gear;
 308     FG_M_n_gear =       M_n_gear;
 309     FG_V_dot_north =    V_dot_north;
 310     FG_V_dot_east =     V_dot_east;
 311     FG_V_dot_down =     V_dot_down;
 312     FG_U_dot_body =     U_dot_body;
 313     FG_V_dot_body =     V_dot_body;
 314     FG_W_dot_body =     W_dot_body;
 315     FG_A_X_cg = A_X_cg;
 316     FG_A_Y_cg = A_Y_cg;
 317     FG_A_Z_cg = A_Z_cg;
 318     FG_A_X_pilot =      A_X_pilot;
 319     FG_A_Y_pilot =      A_Y_pilot;
 320     FG_A_Z_pilot =      A_Z_pilot;
 321     FG_N_X_cg = N_X_cg;
 322     FG_N_Y_cg = N_Y_cg;
 323     FG_N_Z_cg = N_Z_cg;
 324     FG_N_X_pilot =      N_X_pilot;
 325     FG_N_Y_pilot =      N_Y_pilot;
 326     FG_N_Z_pilot =      N_Z_pilot;
 327     FG_P_dot_body =     P_dot_body;
 328     FG_Q_dot_body =     Q_dot_body;
 329     FG_R_dot_body =     R_dot_body;
 330     FG_V_north =        V_north;
 331     FG_V_east = V_east;
 332     FG_V_down = V_down;
 333     FG_V_north_rel_ground =     V_north_rel_ground;
 334     FG_V_east_rel_ground =      V_east_rel_ground;
 335     FG_V_down_rel_ground =      V_down_rel_ground;
 336     FG_V_north_airmass =        V_north_airmass;
 337     FG_V_east_airmass = V_east_airmass;
 338     FG_V_down_airmass = V_down_airmass;
 339     FG_V_north_rel_airmass =    V_north_rel_airmass;
 340     FG_V_east_rel_airmass =     V_east_rel_airmass;
 341     FG_V_down_rel_airmass =     V_down_rel_airmass;
 342     FG_U_gust = U_gust;
 343     FG_V_gust = V_gust;
 344     FG_W_gust = W_gust;
 345     FG_U_body = U_body;
 346     FG_V_body = V_body;
 347     FG_W_body = W_body;
 348     FG_V_rel_wind =     V_rel_wind;
 349     FG_V_true_kts =     V_true_kts;
 350     FG_V_rel_ground =   V_rel_ground;
 351     FG_V_inertial =     V_inertial;
 352     FG_V_ground_speed = V_ground_speed;
 353     FG_V_equiv =        V_equiv;
 354     FG_V_equiv_kts =    V_equiv_kts;
 355     FG_V_calibrated =   V_calibrated;
 356     FG_V_calibrated_kts =       V_calibrated_kts;
 357     FG_P_body = P_body;
 358     FG_Q_body = Q_body;
 359     FG_R_body = R_body;
 360     FG_P_local =        P_local;
 361     FG_Q_local =        Q_local;
 362     FG_R_local =        R_local;
 363     FG_P_total =        P_total;
 364     FG_Q_total =        Q_total;
 365     FG_R_total =        R_total;
 366     FG_Phi_dot =        Phi_dot;
 367     FG_Theta_dot =      Theta_dot;
 368     FG_Psi_dot =        Psi_dot;
 369     FG_Latitude_dot =   Latitude_dot;
 370     FG_Longitude_dot =  Longitude_dot;
 371     FG_Radius_dot =     Radius_dot;
 372     FG_Lat_geocentric = Lat_geocentric;
 373     FG_Lon_geocentric = Lon_geocentric;
 374     FG_Radius_to_vehicle =      Radius_to_vehicle;
 375     FG_Latitude =       Latitude;
 376     FG_Longitude =      Longitude;
 377     FG_Altitude =       Altitude;
 378     FG_Phi =    Phi;
 379     FG_Theta =  Theta;
 380     FG_Psi =    Psi;
 381     FG_T_local_to_body_11 =     T_local_to_body_11;
 382     FG_T_local_to_body_12 =     T_local_to_body_12;
 383     FG_T_local_to_body_13 =     T_local_to_body_13;
 384     FG_T_local_to_body_21 =     T_local_to_body_21;
 385     FG_T_local_to_body_22 =     T_local_to_body_22;
 386     FG_T_local_to_body_23 =     T_local_to_body_23;
 387     FG_T_local_to_body_31 =     T_local_to_body_31;
 388     FG_T_local_to_body_32 =     T_local_to_body_32;
 389     FG_T_local_to_body_33 =     T_local_to_body_33;
 390     FG_Gravity =        Gravity;
 391     FG_Centrifugal_relief =     Centrifugal_relief;
 392     FG_Alpha =  Alpha;
 393     FG_Beta =   Beta;
 394     FG_Alpha_dot =      Alpha_dot;
 395     FG_Beta_dot =       Beta_dot;
 396     FG_Cos_alpha =      Cos_alpha;
 397     FG_Sin_alpha =      Sin_alpha;
 398     FG_Cos_beta =       Cos_beta;
 399     FG_Sin_beta =       Sin_beta;
 400     FG_Cos_phi =        Cos_phi;
 401     FG_Sin_phi =        Sin_phi;
 402     FG_Cos_theta =      Cos_theta;
 403     FG_Sin_theta =      Sin_theta;
 404     FG_Cos_psi =        Cos_psi;
 405     FG_Sin_psi =        Sin_psi;
 406     FG_Gamma_vert_rad = Gamma_vert_rad;
 407     FG_Gamma_horiz_rad =        Gamma_horiz_rad;
 408     FG_Sigma =  Sigma;
 409     FG_Density =        Density;
 410     FG_V_sound =        V_sound;
 411     FG_Mach_number =    Mach_number;
 412     FG_Static_pressure =        Static_pressure;
 413     FG_Total_pressure = Total_pressure;
 414     FG_Impact_pressure =        Impact_pressure;
 415     FG_Dynamic_pressure =       Dynamic_pressure;
 416     FG_Static_temperature =     Static_temperature;
 417     FG_Total_temperature =      Total_temperature;
 418     FG_Sea_level_radius =       Sea_level_radius;
 419     FG_Earth_position_angle =   Earth_position_angle;
 420     FG_Runway_altitude =        Runway_altitude;
 421     FG_Runway_latitude =        Runway_latitude;
 422     FG_Runway_longitude =       Runway_longitude;
 423     FG_Runway_heading = Runway_heading;
 424     FG_Radius_to_rwy =  Radius_to_rwy;
 425     FG_D_cg_north_of_rwy =      D_cg_north_of_rwy;
 426     FG_D_cg_east_of_rwy =       D_cg_east_of_rwy;
 427     FG_D_cg_above_rwy = D_cg_above_rwy;
 428     FG_X_cg_rwy =       X_cg_rwy;
 429     FG_Y_cg_rwy =       Y_cg_rwy;
 430     FG_H_cg_rwy =       H_cg_rwy;
 431     FG_D_pilot_north_of_rwy =   D_pilot_north_of_rwy;
 432     FG_D_pilot_east_of_rwy =    D_pilot_east_of_rwy;
 433     FG_D_pilot_above_rwy =      D_pilot_above_rwy;
 434     FG_X_pilot_rwy =    X_pilot_rwy;
 435     FG_Y_pilot_rwy =    Y_pilot_rwy;
 436     FG_H_pilot_rwy =    H_pilot_rwy;
 437
 438     return ( 0 );
 439 }
 440
 441
 442 // $Log$
 443 // Revision 1.2  1998/10/17 01:34:11  curt
 444 // C++ ifying ...
 445 //
 446 // Revision 1.1  1998/10/17 00:43:58  curt
 447 // Initial revision.
 448 //
 449 //