diff --git a/trick_models/parachute/include/Parachutist.hh b/trick_models/parachute/include/Parachutist.hh new file mode 100644 index 00000000..b20f8786 --- /dev/null +++ b/trick_models/parachute/include/Parachutist.hh @@ -0,0 +1,49 @@ +/************************************************************************* +PURPOSE: (Parachutist Structure) +LIBRARY DEPENDENCIES: + ( + (parachutist/src/Parachutist.o) + ) + +**************************************************************************/ + +#ifndef _parachutist_hh_ +#define _parachutist_hh_ +#include "sim_services/Integrator/include/regula_falsi.h" + +class Parachutist { + + public: + double position ; /* m xyz-position */ + double velocity ; /* m/s xyz-velocity */ + double acceleration ; /* m/s2 xyz-acceleration */ + + double crossSectionalArea; /* m2 */ + double crossSectionalAreaRate; /* m2/s */ + double cooefficientOfDrag; /* -- */ + double cooefficientOfDragRate; /* -- */ + double mass; /* kg */ + + double parachuteDeploymentStartTime; /* s */ + double parachuteDeploymentDuration; /* s */ + + static const double crossSectionalArea_freefall; /* m2 */ + static const double crossSectionalArea_parachute; /* m2 */ + static const double cooefficientOfDrag_freefall; /* -- */ + static const double cooefficientOfDrag_parachute; /* -- */ + + bool touchDown; /* -- */ + + REGULA_FALSI rf ; + + int default_data(); + int state_init(); + int state_deriv(); + int state_integ(); + int parachute_control(); + double touch_down(); + private: + double parachuteDeploymentEndTime; /* s */ +}; + +#endif diff --git a/trick_models/parachute/include/interpolate.h b/trick_models/parachute/include/interpolate.h new file mode 100644 index 00000000..4ba3a23b --- /dev/null +++ b/trick_models/parachute/include/interpolate.h @@ -0,0 +1,15 @@ +#ifndef INTERPOLATE_H +#define INTERPOLATE_H + +#ifdef __cplusplus +extern "C" { +#endif + +double interpolate( double x, const double xa[], const double fa[], int N_elems ); + +#ifdef __cplusplus +} +#endif + +#endif + diff --git a/trick_models/parachute/src/Parachutist.cpp b/trick_models/parachute/src/Parachutist.cpp new file mode 100644 index 00000000..caacb53d --- /dev/null +++ b/trick_models/parachute/src/Parachutist.cpp @@ -0,0 +1,160 @@ +/********************************* TRICK HEADER ******************************* +PURPOSE: ( Simulate a skydiver jumping from very high altitude.) +LIBRARY DEPENDENCY: + ((Parachutist.o)(interpolate.o)) +PROGRAMMERS: + (((John M. Penn) (L3 Communications) (March 2015) (Trick Refresher Project))) +*******************************************************************************/ +#include "../include/Parachutist.hh" +#include "../include/interpolate.h" +#include + +// The following three data arrays come from: +// http://www.engineeringtoolbox.com/standard-atmosphere-d_604.html +// U.S Standard Atmosphere Air Properties in SI Units +#define NUM_ELEMENTS 21 + +// Units = meters (above sea level). +const double altitude_array[NUM_ELEMENTS] = { + -1000.0, 0.0, 1000.0, 2000.0, 3000.0, 4000.0, 5000.0, 6000.0, + 7000.0, 8000.0, 9000.0, 10000.0, 15000.0, 20000.0, 25000.0, 30000.0, + 40000.0, 50000.0, 60000.0, 70000.0, 80000.0 }; + +// Units = kilograms per cubic meter. +const double air_density_array[NUM_ELEMENTS] = { + 1.347, 1.225, 1.112, 1.007, 0.9093, 0.8194, 0.7364, 0.6601, + 0.5900, 0.5258, 0.4671, 0.4135, 0.1948, 0.08891, 0.04008, 0.01841, + 0.003996, 0.001027, 0.0003097, 0.00008283, 0.00001846 }; + +// Units = meters per second squared. +const double gravity_array[NUM_ELEMENTS] = { +9.810, 9.807, 9.804, 9.801, 9.797, 9.794, 9.791, 9.788, +9.785, 9.782, 9.779, 9.776, 9.761, 9.745, 9.730, 9.715, +9.684, 9.654, 9.624, 9.594, 9.564 +}; + +const double Parachutist::crossSectionalArea_freefall = 0.75 ; +const double Parachutist::crossSectionalArea_parachute = 30.00; +const double Parachutist::cooefficientOfDrag_freefall = 0.75; +const double Parachutist::cooefficientOfDrag_parachute = 1.30; + +int Parachutist::default_data() { + + position = 38969.3; //38969.3 meters = 127852 feet + velocity = 0.0; + acceleration = 0.0; + parachuteDeploymentStartTime = 259; /* 4 minutes, 19 seconds*/ + parachuteDeploymentDuration = 3; + crossSectionalArea = crossSectionalArea_freefall; + cooefficientOfDrag = cooefficientOfDrag_freefall; + touchDown = false; + mass = 82.0; + + return (0); +} + +int Parachutist::state_init() { + parachuteDeploymentEndTime = parachuteDeploymentStartTime + parachuteDeploymentDuration; + return (0); +} + +#include "sim_services/Executive/include/exec_proto.h" +int Parachutist::parachute_control() { + + double currentTime = exec_get_sim_time(); + + if ((currentTime > parachuteDeploymentStartTime) && + (currentTime <= parachuteDeploymentEndTime)) { + cooefficientOfDragRate = (cooefficientOfDrag_parachute - cooefficientOfDrag_freefall) + / parachuteDeploymentDuration; + crossSectionalAreaRate = (crossSectionalArea_parachute - crossSectionalArea_freefall) + / parachuteDeploymentDuration; + } else { + cooefficientOfDragRate = 0.0; + crossSectionalAreaRate = 0.0; + } + return (0); +} + +int Parachutist::state_deriv() { + + // Calculate the force of gravity. +#if 1 + double g = 9.81; +#else + double g = interpolate( position, altitude_array, gravity_array, NUM_ELEMENTS ); +#endif + + double Force_gravity = mass * -g; + + // Calculate the force of drag. + double air_density = interpolate( position, altitude_array, air_density_array, NUM_ELEMENTS ); + double Force_drag = cooefficientOfDrag * 0.5 * air_density * velocity * velocity * crossSectionalArea; + + // Sum the forces and calculate acceleration. + double Force_total; + + // If skydiver has touched down then set state derivatives to zero. + if ( touchDown ) { + Force_total = 0.0; + velocity = 0.0; + acceleration = 0.0; + } else { + Force_total = Force_gravity + Force_drag ; + acceleration = Force_total / mass; + } + + /* RETURN: -- Always return zero. */ + return(0); +} + +#include "sim_services/Integrator/include/integrator_c_intf.h" +int Parachutist::state_integ() { + + int integration_step; + + load_state( &position, + &velocity, + &cooefficientOfDrag, + &crossSectionalArea, + (double*)0 + ); + + load_deriv ( &velocity, + &acceleration, + &cooefficientOfDragRate, + &crossSectionalAreaRate, + (double*)0 + ); + + integration_step = integrate(); + + unload_state( &position, + &velocity, + &cooefficientOfDrag, + &crossSectionalArea, + (double*)0 + ); + + return(integration_step); +} + +double Parachutist::touch_down() { + double tgo ; + double now ; + + /* error function: how far the skydiver is above ground */ + rf.error = position ; + + now = get_integ_time() ; + tgo = regula_falsi( now, &rf ) ; + if (tgo == 0.0) { + now = get_integ_time() ; + reset_regula_falsi( now, &rf); + velocity = 0.0; + acceleration = 0.0; + touchDown = true; + std::cout << "Touchdown: time = " << now << "." << std::endl; + } + return ( tgo ) ; +} diff --git a/trick_models/parachute/src/interpolate b/trick_models/parachute/src/interpolate new file mode 100755 index 00000000..a461ad1c Binary files /dev/null and b/trick_models/parachute/src/interpolate differ diff --git a/trick_models/parachute/src/interpolate.cpp b/trick_models/parachute/src/interpolate.cpp new file mode 100644 index 00000000..3858108b --- /dev/null +++ b/trick_models/parachute/src/interpolate.cpp @@ -0,0 +1,22 @@ +#include "../include/interpolate.h" +#include + +double interpolate( double x, const double xa[], const double fa[], int N_elems ) { + int ii; + for (ii=0 ; ((ii+2 < N_elems ) && (x > xa[ii+1])) ; ii++ ) ; + double x_lower = xa[ii]; + double x_upper = xa[ii+1]; + double f_lower = fa[ii]; + double f_upper = fa[ii+1]; + if (x < x_lower) { + fprintf(stderr, "Interpolation x is out of range low.\n"); + return(f_lower); + } + if (x > x_upper) { + fprintf(stderr, "Interpolation x is out of range high.\n"); + return(f_upper); + } + double f = (x_upper - x)/(x_upper - x_lower) * f_lower + + (x - x_lower)/(x_upper - x_lower) * f_upper ; + return(f); +} diff --git a/trick_sims/SIM_parachute/DP_Product/DP_parachutist.xml b/trick_sims/SIM_parachute/DP_Product/DP_parachutist.xml new file mode 100644 index 00000000..5a4984d2 --- /dev/null +++ b/trick_sims/SIM_parachute/DP_Product/DP_parachutist.xml @@ -0,0 +1,47 @@ + + + + -1.0E20 + 1.0E20 + 0.0 + + Page + + Plot + + sys.exec.out.time + dyn.parachutist.position + + + + + Page + + Plot + + sys.exec.out.time + dyn.parachutist.velocity + + + + + Page + + Plot + + sys.exec.out.time + dyn.parachutist.acceleration + + + + + Page + + Plot + + sys.exec.out.time + dyn.parachutist.cooefficientOfDrag + + + + diff --git a/trick_sims/SIM_parachute/Modified_data/parachutist.dr b/trick_sims/SIM_parachute/Modified_data/parachutist.dr new file mode 100644 index 00000000..e077b4ad --- /dev/null +++ b/trick_sims/SIM_parachute/Modified_data/parachutist.dr @@ -0,0 +1,20 @@ +global DR_GROUP_ID +global drg +try: + if DR_GROUP_ID >= 0: + DR_GROUP_ID += 1 +except NameError: + DR_GROUP_ID = 0 + drg = [] + +drg.append(trick.DRBinary("parachutist")) +drg[DR_GROUP_ID].set_freq(trick.DR_Always) +drg[DR_GROUP_ID].set_cycle(0.1) +drg[DR_GROUP_ID].set_single_prec_only(False) +drg[DR_GROUP_ID].add_variable("dyn.parachutist.position") +drg[DR_GROUP_ID].add_variable("dyn.parachutist.velocity") +drg[DR_GROUP_ID].add_variable("dyn.parachutist.acceleration") +drg[DR_GROUP_ID].add_variable("dyn.parachutist.crossSectionalArea") +drg[DR_GROUP_ID].add_variable("dyn.parachutist.cooefficientOfDrag") +trick.add_data_record_group(drg[DR_GROUP_ID], trick.DR_Buffer) +drg[DR_GROUP_ID].enable() diff --git a/trick_sims/SIM_parachute/RUN_Baumgartner_jump/input.py b/trick_sims/SIM_parachute/RUN_Baumgartner_jump/input.py new file mode 100644 index 00000000..41719761 --- /dev/null +++ b/trick_sims/SIM_parachute/RUN_Baumgartner_jump/input.py @@ -0,0 +1,7 @@ +#execfile("Modified_data/realtime.py") +execfile("Modified_data/parachutist.dr") + +trick.TMM_reduced_checkpoint(0) +dyn_integloop.getIntegrator(trick.Runge_Kutta_4, 4) + +trick.stop(800) diff --git a/trick_sims/SIM_parachute/S_define b/trick_sims/SIM_parachute/S_define new file mode 100644 index 00000000..0ae83bd1 --- /dev/null +++ b/trick_sims/SIM_parachute/S_define @@ -0,0 +1,30 @@ +/************************TRICK HEADER************************* +PURPOSE: + ( Simulate a skydiver jump from very high altitude. ) +LIBRARY DEPENDENCIES: + ((parachute/src/Parachutist.cpp)) +*************************************************************/ + +#include "sim_objects/default_trick_sys.sm" + +##include "parachute/include/Parachutist.hh" + +class ParachutistSimObject : public Trick::SimObject { + + public: + Parachutist parachutist; + + ParachutistSimObject() { + + ("default_data") parachutist.default_data() ; + ("initialization") parachutist.state_init() ; + ("derivative") parachutist.state_deriv() ; + ("integration") trick_ret = parachutist.state_integ() ; + ("dynamic_event") parachutist.touch_down() ; + (0.02, "scheduled") parachutist.parachute_control(); + } +} ; + +ParachutistSimObject dyn ; + +IntegLoop dyn_integloop(0.02) dyn; diff --git a/trick_sims/SIM_parachute/S_overrides.mk b/trick_sims/SIM_parachute/S_overrides.mk new file mode 100644 index 00000000..8fa02be5 --- /dev/null +++ b/trick_sims/SIM_parachute/S_overrides.mk @@ -0,0 +1,4 @@ + +TRICK_CFLAGS += -I${TRICK_HOME}/trick_models +TRICK_CXXFLAGS += -I${TRICK_HOME}/trick_models +