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.h>
  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     Mass =      FG_Mass;
  82     I_xx =      FG_I_xx;
  83     I_yy =      FG_I_yy;
  84     I_zz =      FG_I_zz;
  85     I_xz =      FG_I_xz;
  86     Dx_pilot =  FG_Dx_pilot;
  87     Dy_pilot =  FG_Dy_pilot;
  88     Dz_pilot =  FG_Dz_pilot;
  89     Dx_cg =     FG_Dx_cg;
  90     Dy_cg =     FG_Dy_cg;
  91     Dz_cg =     FG_Dz_cg;
  92     F_X =       FG_F_X;
  93     F_Y =       FG_F_Y;
  94     F_Z =       FG_F_Z;
  95     F_north =   FG_F_north;
  96     F_east =    FG_F_east;
  97     F_down =    FG_F_down;
  98     F_X_aero =  FG_F_X_aero;
  99     F_Y_aero =  FG_F_Y_aero;
 100     F_Z_aero =  FG_F_Z_aero;
 101     F_X_engine =        FG_F_X_engine;
 102     F_Y_engine =        FG_F_Y_engine;
 103     F_Z_engine =        FG_F_Z_engine;
 104     F_X_gear =  FG_F_X_gear;
 105     F_Y_gear =  FG_F_Y_gear;
 106     F_Z_gear =  FG_F_Z_gear;
 107     M_l_rp =    FG_M_l_rp;
 108     M_m_rp =    FG_M_m_rp;
 109     M_n_rp =    FG_M_n_rp;
 110     M_l_cg =    FG_M_l_cg;
 111     M_m_cg =    FG_M_m_cg;
 112     M_n_cg =    FG_M_n_cg;
 113     M_l_aero =  FG_M_l_aero;
 114     M_m_aero =  FG_M_m_aero;
 115     M_n_aero =  FG_M_n_aero;
 116     M_l_engine =        FG_M_l_engine;
 117     M_m_engine =        FG_M_m_engine;
 118     M_n_engine =        FG_M_n_engine;
 119     M_l_gear =  FG_M_l_gear;
 120     M_m_gear =  FG_M_m_gear;
 121     M_n_gear =  FG_M_n_gear;
 122     V_dot_north =       FG_V_dot_north;
 123     V_dot_east =        FG_V_dot_east;
 124     V_dot_down =        FG_V_dot_down;
 125     U_dot_body =        FG_U_dot_body;
 126     V_dot_body =        FG_V_dot_body;
 127     W_dot_body =        FG_W_dot_body;
 128     A_X_cg =    FG_A_X_cg;
 129     A_Y_cg =    FG_A_Y_cg;
 130     A_Z_cg =    FG_A_Z_cg;
 131     A_X_pilot = FG_A_X_pilot;
 132     A_Y_pilot = FG_A_Y_pilot;
 133     A_Z_pilot = FG_A_Z_pilot;
 134     N_X_cg =    FG_N_X_cg;
 135     N_Y_cg =    FG_N_Y_cg;
 136     N_Z_cg =    FG_N_Z_cg;
 137     N_X_pilot = FG_N_X_pilot;
 138     N_Y_pilot = FG_N_Y_pilot;
 139     N_Z_pilot = FG_N_Z_pilot;
 140     P_dot_body =        FG_P_dot_body;
 141     Q_dot_body =        FG_Q_dot_body;
 142     R_dot_body =        FG_R_dot_body;
 143     V_north =   FG_V_north;
 144     V_east =    FG_V_east;
 145     V_down =    FG_V_down;
 146     V_north_rel_ground =        FG_V_north_rel_ground;
 147     V_east_rel_ground = FG_V_east_rel_ground;
 148     V_down_rel_ground = FG_V_down_rel_ground;
 149     V_north_airmass =   FG_V_north_airmass;
 150     V_east_airmass =    FG_V_east_airmass;
 151     V_down_airmass =    FG_V_down_airmass;
 152     V_north_rel_airmass =       FG_V_north_rel_airmass;
 153     V_east_rel_airmass =        FG_V_east_rel_airmass;
 154     V_down_rel_airmass =        FG_V_down_rel_airmass;
 155     U_gust =    FG_U_gust;
 156     V_gust =    FG_V_gust;
 157     W_gust =    FG_W_gust;
 158     U_body =    FG_U_body;
 159     V_body =    FG_V_body;
 160     W_body =    FG_W_body;
 161     V_rel_wind =        FG_V_rel_wind;
 162     V_true_kts =        FG_V_true_kts;
 163     V_rel_ground =      FG_V_rel_ground;
 164     V_inertial =        FG_V_inertial;
 165     V_ground_speed =    FG_V_ground_speed;
 166     V_equiv =   FG_V_equiv;
 167     V_equiv_kts =       FG_V_equiv_kts;
 168     V_calibrated =      FG_V_calibrated;
 169     V_calibrated_kts =  FG_V_calibrated_kts;
 170     P_body =    FG_P_body;
 171     Q_body =    FG_Q_body;
 172     R_body =    FG_R_body;
 173     P_local =   FG_P_local;
 174     Q_local =   FG_Q_local;
 175     R_local =   FG_R_local;
 176     P_total =   FG_P_total;
 177     Q_total =   FG_Q_total;
 178     R_total =   FG_R_total;
 179     Phi_dot =   FG_Phi_dot;
 180     Theta_dot = FG_Theta_dot;
 181     Psi_dot =   FG_Psi_dot;
 182     Latitude_dot =      FG_Latitude_dot;
 183     Longitude_dot =     FG_Longitude_dot;
 184     Radius_dot =        FG_Radius_dot;
 185     Lat_geocentric =    FG_Lat_geocentric;
 186     Lon_geocentric =    FG_Lon_geocentric;
 187     Radius_to_vehicle = FG_Radius_to_vehicle;
 188     Latitude =  FG_Latitude;
 189     Longitude = FG_Longitude;
 190     Altitude =  FG_Altitude;
 191     Phi =       FG_Phi;
 192     Theta =     FG_Theta;
 193     Psi =       FG_Psi;
 194     T_local_to_body_11 =        FG_T_local_to_body_11;
 195     T_local_to_body_12 =        FG_T_local_to_body_12;
 196     T_local_to_body_13 =        FG_T_local_to_body_13;
 197     T_local_to_body_21 =        FG_T_local_to_body_21;
 198     T_local_to_body_22 =        FG_T_local_to_body_22;
 199     T_local_to_body_23 =        FG_T_local_to_body_23;
 200     T_local_to_body_31 =        FG_T_local_to_body_31;
 201     T_local_to_body_32 =        FG_T_local_to_body_32;
 202     T_local_to_body_33 =        FG_T_local_to_body_33;
 203     Gravity =   FG_Gravity;
 204     Centrifugal_relief =        FG_Centrifugal_relief;
 205     Alpha =     FG_Alpha;
 206     Beta =      FG_Beta;
 207     Alpha_dot = FG_Alpha_dot;
 208     Beta_dot =  FG_Beta_dot;
 209     Cos_alpha = FG_Cos_alpha;
 210     Sin_alpha = FG_Sin_alpha;
 211     Cos_beta =  FG_Cos_beta;
 212     Sin_beta =  FG_Sin_beta;
 213     Cos_phi =   FG_Cos_phi;
 214     Sin_phi =   FG_Sin_phi;
 215     Cos_theta = FG_Cos_theta;
 216     Sin_theta = FG_Sin_theta;
 217     Cos_psi =   FG_Cos_psi;
 218     Sin_psi =   FG_Sin_psi;
 219     Gamma_vert_rad =    FG_Gamma_vert_rad;
 220     Gamma_horiz_rad =   FG_Gamma_horiz_rad;
 221     Sigma =     FG_Sigma;
 222     Density =   FG_Density;
 223     V_sound =   FG_V_sound;
 224     Mach_number =       FG_Mach_number;
 225     Static_pressure =   FG_Static_pressure;
 226     Total_pressure =    FG_Total_pressure;
 227     Impact_pressure =   FG_Impact_pressure;
 228     Dynamic_pressure =  FG_Dynamic_pressure;
 229     Static_temperature =        FG_Static_temperature;
 230     Total_temperature = FG_Total_temperature;
 231     Sea_level_radius =  FG_Sea_level_radius;
 232     Earth_position_angle =      FG_Earth_position_angle;
 233     Runway_altitude =   FG_Runway_altitude;
 234     Runway_latitude =   FG_Runway_latitude;
 235     Runway_longitude =  FG_Runway_longitude;
 236     Runway_heading =    FG_Runway_heading;
 237     Radius_to_rwy =     FG_Radius_to_rwy;
 238     D_cg_north_of_rwy = FG_D_cg_north_of_rwy;
 239     D_cg_east_of_rwy =  FG_D_cg_east_of_rwy;
 240     D_cg_above_rwy =    FG_D_cg_above_rwy;
 241     X_cg_rwy =  FG_X_cg_rwy;
 242     Y_cg_rwy =  FG_Y_cg_rwy;
 243     H_cg_rwy =  FG_H_cg_rwy;
 244     D_pilot_north_of_rwy =      FG_D_pilot_north_of_rwy;
 245     D_pilot_east_of_rwy =       FG_D_pilot_east_of_rwy;
 246     D_pilot_above_rwy = FG_D_pilot_above_rwy;
 247     X_pilot_rwy =       FG_X_pilot_rwy;
 248     Y_pilot_rwy =       FG_Y_pilot_rwy;
 249     H_pilot_rwy =       FG_H_pilot_rwy;
 250
 251     return( 0 );
 252 }
 253
 254
 255 // Convert from the LaRCsim generic_ struct to the fgFLIGHT struct
 256 int fgLaRCsim_2_Flight (fgFLIGHT *f) {
 257     fgCONTROLS *c;
 258
 259     c = current_aircraft.controls;
 260
 261     Lat_control = FG_Aileron;
 262     Long_control = FG_Elevator;
 263     Long_trim = FG_Elev_Trim;
 264     Rudder_pedal = FG_Rudder;
 265     Throttle_pct = FG_Throttle[0];
 266
 267     FG_Mass =   Mass;
 268     FG_I_xx =   I_xx;
 269     FG_I_yy =   I_yy;
 270     FG_I_zz =   I_zz;
 271     FG_I_xz =   I_xz;
 272     FG_Dx_pilot =       Dx_pilot;
 273     FG_Dy_pilot =       Dy_pilot;
 274     FG_Dz_pilot =       Dz_pilot;
 275     FG_Dx_cg =  Dx_cg;
 276     FG_Dy_cg =  Dy_cg;
 277     FG_Dz_cg =  Dz_cg;
 278     FG_F_X =    F_X;
 279     FG_F_Y =    F_Y;
 280     FG_F_Z =    F_Z;
 281     FG_F_north =        F_north;
 282     FG_F_east = F_east;
 283     FG_F_down = F_down;
 284     FG_F_X_aero =       F_X_aero;
 285     FG_F_Y_aero =       F_Y_aero;
 286     FG_F_Z_aero =       F_Z_aero;
 287     FG_F_X_engine =     F_X_engine;
 288     FG_F_Y_engine =     F_Y_engine;
 289     FG_F_Z_engine =     F_Z_engine;
 290     FG_F_X_gear =       F_X_gear;
 291     FG_F_Y_gear =       F_Y_gear;
 292     FG_F_Z_gear =       F_Z_gear;
 293     FG_M_l_rp = M_l_rp;
 294     FG_M_m_rp = M_m_rp;
 295     FG_M_n_rp = M_n_rp;
 296     FG_M_l_cg = M_l_cg;
 297     FG_M_m_cg = M_m_cg;
 298     FG_M_n_cg = M_n_cg;
 299     FG_M_l_aero =       M_l_aero;
 300     FG_M_m_aero =       M_m_aero;
 301     FG_M_n_aero =       M_n_aero;
 302     FG_M_l_engine =     M_l_engine;
 303     FG_M_m_engine =     M_m_engine;
 304     FG_M_n_engine =     M_n_engine;
 305     FG_M_l_gear =       M_l_gear;
 306     FG_M_m_gear =       M_m_gear;
 307     FG_M_n_gear =       M_n_gear;
 308     FG_V_dot_north =    V_dot_north;
 309     FG_V_dot_east =     V_dot_east;
 310     FG_V_dot_down =     V_dot_down;
 311     FG_U_dot_body =     U_dot_body;
 312     FG_V_dot_body =     V_dot_body;
 313     FG_W_dot_body =     W_dot_body;
 314     FG_A_X_cg = A_X_cg;
 315     FG_A_Y_cg = A_Y_cg;
 316     FG_A_Z_cg = A_Z_cg;
 317     FG_A_X_pilot =      A_X_pilot;
 318     FG_A_Y_pilot =      A_Y_pilot;
 319     FG_A_Z_pilot =      A_Z_pilot;
 320     FG_N_X_cg = N_X_cg;
 321     FG_N_Y_cg = N_Y_cg;
 322     FG_N_Z_cg = N_Z_cg;
 323     FG_N_X_pilot =      N_X_pilot;
 324     FG_N_Y_pilot =      N_Y_pilot;
 325     FG_N_Z_pilot =      N_Z_pilot;
 326     FG_P_dot_body =     P_dot_body;
 327     FG_Q_dot_body =     Q_dot_body;
 328     FG_R_dot_body =     R_dot_body;
 329     FG_V_north =        V_north;
 330     FG_V_east = V_east;
 331     FG_V_down = V_down;
 332     FG_V_north_rel_ground =     V_north_rel_ground;
 333     FG_V_east_rel_ground =      V_east_rel_ground;
 334     FG_V_down_rel_ground =      V_down_rel_ground;
 335     FG_V_north_airmass =        V_north_airmass;
 336     FG_V_east_airmass = V_east_airmass;
 337     FG_V_down_airmass = V_down_airmass;
 338     FG_V_north_rel_airmass =    V_north_rel_airmass;
 339     FG_V_east_rel_airmass =     V_east_rel_airmass;
 340     FG_V_down_rel_airmass =     V_down_rel_airmass;
 341     FG_U_gust = U_gust;
 342     FG_V_gust = V_gust;
 343     FG_W_gust = W_gust;
 344     FG_U_body = U_body;
 345     FG_V_body = V_body;
 346     FG_W_body = W_body;
 347     FG_V_rel_wind =     V_rel_wind;
 348     FG_V_true_kts =     V_true_kts;
 349     FG_V_rel_ground =   V_rel_ground;
 350     FG_V_inertial =     V_inertial;
 351     FG_V_ground_speed = V_ground_speed;
 352     FG_V_equiv =        V_equiv;
 353     FG_V_equiv_kts =    V_equiv_kts;
 354     FG_V_calibrated =   V_calibrated;
 355     FG_V_calibrated_kts =       V_calibrated_kts;
 356     FG_P_body = P_body;
 357     FG_Q_body = Q_body;
 358     FG_R_body = R_body;
 359     FG_P_local =        P_local;
 360     FG_Q_local =        Q_local;
 361     FG_R_local =        R_local;
 362     FG_P_total =        P_total;
 363     FG_Q_total =        Q_total;
 364     FG_R_total =        R_total;
 365     FG_Phi_dot =        Phi_dot;
 366     FG_Theta_dot =      Theta_dot;
 367     FG_Psi_dot =        Psi_dot;
 368     FG_Latitude_dot =   Latitude_dot;
 369     FG_Longitude_dot =  Longitude_dot;
 370     FG_Radius_dot =     Radius_dot;
 371     FG_Lat_geocentric = Lat_geocentric;
 372     FG_Lon_geocentric = Lon_geocentric;
 373     FG_Radius_to_vehicle =      Radius_to_vehicle;
 374     FG_Latitude =       Latitude;
 375     FG_Longitude =      Longitude;
 376     FG_Altitude =       Altitude;
 377     FG_Phi =    Phi;
 378     FG_Theta =  Theta;
 379     FG_Psi =    Psi;
 380     FG_T_local_to_body_11 =     T_local_to_body_11;
 381     FG_T_local_to_body_12 =     T_local_to_body_12;
 382     FG_T_local_to_body_13 =     T_local_to_body_13;
 383     FG_T_local_to_body_21 =     T_local_to_body_21;
 384     FG_T_local_to_body_22 =     T_local_to_body_22;
 385     FG_T_local_to_body_23 =     T_local_to_body_23;
 386     FG_T_local_to_body_31 =     T_local_to_body_31;
 387     FG_T_local_to_body_32 =     T_local_to_body_32;
 388     FG_T_local_to_body_33 =     T_local_to_body_33;
 389     FG_Gravity =        Gravity;
 390     FG_Centrifugal_relief =     Centrifugal_relief;
 391     FG_Alpha =  Alpha;
 392     FG_Beta =   Beta;
 393     FG_Alpha_dot =      Alpha_dot;
 394     FG_Beta_dot =       Beta_dot;
 395     FG_Cos_alpha =      Cos_alpha;
 396     FG_Sin_alpha =      Sin_alpha;
 397     FG_Cos_beta =       Cos_beta;
 398     FG_Sin_beta =       Sin_beta;
 399     FG_Cos_phi =        Cos_phi;
 400     FG_Sin_phi =        Sin_phi;
 401     FG_Cos_theta =      Cos_theta;
 402     FG_Sin_theta =      Sin_theta;
 403     FG_Cos_psi =        Cos_psi;
 404     FG_Sin_psi =        Sin_psi;
 405     FG_Gamma_vert_rad = Gamma_vert_rad;
 406     FG_Gamma_horiz_rad =        Gamma_horiz_rad;
 407     FG_Sigma =  Sigma;
 408     FG_Density =        Density;
 409     FG_V_sound =        V_sound;
 410     FG_Mach_number =    Mach_number;
 411     FG_Static_pressure =        Static_pressure;
 412     FG_Total_pressure = Total_pressure;
 413     FG_Impact_pressure =        Impact_pressure;
 414     FG_Dynamic_pressure =       Dynamic_pressure;
 415     FG_Static_temperature =     Static_temperature;
 416     FG_Total_temperature =      Total_temperature;
 417     FG_Sea_level_radius =       Sea_level_radius;
 418     FG_Earth_position_angle =   Earth_position_angle;
 419     FG_Runway_altitude =        Runway_altitude;
 420     FG_Runway_latitude =        Runway_latitude;
 421     FG_Runway_longitude =       Runway_longitude;
 422     FG_Runway_heading = Runway_heading;
 423     FG_Radius_to_rwy =  Radius_to_rwy;
 424     FG_D_cg_north_of_rwy =      D_cg_north_of_rwy;
 425     FG_D_cg_east_of_rwy =       D_cg_east_of_rwy;
 426     FG_D_cg_above_rwy = D_cg_above_rwy;
 427     FG_X_cg_rwy =       X_cg_rwy;
 428     FG_Y_cg_rwy =       Y_cg_rwy;
 429     FG_H_cg_rwy =       H_cg_rwy;
 430     FG_D_pilot_north_of_rwy =   D_pilot_north_of_rwy;
 431     FG_D_pilot_east_of_rwy =    D_pilot_east_of_rwy;
 432     FG_D_pilot_above_rwy =      D_pilot_above_rwy;
 433     FG_X_pilot_rwy =    X_pilot_rwy;
 434     FG_Y_pilot_rwy =    Y_pilot_rwy;
 435     FG_H_pilot_rwy =    H_pilot_rwy;
 436
 437     return ( 0 );
 438 }
 439
 440
 441 // $Log$
 442 // Revision 1.1  1998/10/17 00:43:58  curt
 443 // Initial revision.
 444 //
 445 //