Implement Felix Baumgartner parachutist simulation. Fixes #32

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

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
+

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 <iostream>
+
+// 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 ) ;
+}

trick_models/parachute/src/interpolate.cpp

@@ -0,0 +1,22 @@
+#include "../include/interpolate.h"
+#include <stdio.h>
+
+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);
+}

trick_sims/SIM_parachute/DP_Product/DP_parachutist.xml

@@ -0,0 +1,47 @@
+<?xml version="1.0" encoding="ISO-8859-1" standalone="no"?>
+<!DOCTYPE product PUBLIC "-//Tricklab//DTD Product V1.0//EN" "Product.dtd">
+<product version="1.0">
+    <tstart>-1.0E20</tstart>
+    <tstop>1.0E20</tstop>
+    <frequency>0.0</frequency>
+    <page hcells="0" vcells="0">
+        <title>Page</title>
+        <plot grid="Yes" grid_color="#b2b2b2">
+            <title>Plot</title>
+            <curve>
+                <var units="s">sys.exec.out.time</var>
+                <var units="m">dyn.parachutist.position</var>
+            </curve>
+        </plot>
+    </page>
+    <page hcells="0" vcells="0">
+        <title>Page</title>
+        <plot grid="Yes" grid_color="#b2b2b2">
+            <title>Plot</title>
+            <curve>
+                <var units="s">sys.exec.out.time</var>
+                <var units="m/s">dyn.parachutist.velocity</var>
+            </curve>
+        </plot>
+    </page>
+    <page hcells="0" vcells="0">
+        <title>Page</title>
+        <plot grid="Yes" grid_color="#b2b2b2">
+            <title>Plot</title>
+            <curve>
+                <var units="s">sys.exec.out.time</var>
+                <var units="m/s2">dyn.parachutist.acceleration</var>
+            </curve>
+        </plot>
+    </page>
+    <page hcells="0" vcells="0">
+        <title>Page</title>
+        <plot grid="Yes" grid_color="#b2b2b2">
+            <title>Plot</title>
+            <curve>
+                <var units="s">sys.exec.out.time</var>
+                <var units="--">dyn.parachutist.cooefficientOfDrag</var>
+            </curve>
+        </plot>
+    </page>
+</product>

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()

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)

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;

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
+