Merge pull request #222 from paparazzi/nps_jsbsim_ground_interactions

decreases jsbsim stepsize when close to the ground, so no more bouncing and initialization on the ground is possible.

conf/simulator/jsbsim/aircraft/reset00.xml

@@ -11,10 +11,9 @@
   <!--<latitude  unit="DEG">  37.6136 </latitude> -->
   <longitude unit="DEG">-122.3569 </longitude>
 <!-- altitude is above ground level AGL -->
-<!--  <altitude  unit="M"> 0.11 </altitude> -->
+  <altitude  unit="M"> 0.15 </altitude>
 <!-- altitudeMSL is above sea level ASL -->
-  <altitudeMSL unit="M"> 2.5 </altitudeMSL>
-<!--  <altitude  unit="M">95 </altitude> -->
+<!-- <altitudeMSL unit="M"> 2.5 </altitudeMSL> -->
   <winddir unit="DEG"> 0.0 </winddir>
   <vwind unit="FT/SEC"> 0.0 </vwind>
 </initialize>

sw/simulator/nps/nps_autopilot_rotorcraft.c

@@ -93,18 +93,6 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) {
 
   handle_periodic_tasks();
 
-  /* override the commands for the first 8 seconds...
-   * start with a little bit of hovering
-   */
-  if (time < 8) {
-    int32_t init_cmd[4];
-    init_cmd[COMMAND_THRUST] = 0.253*MAX_PPRZ;
-    init_cmd[COMMAND_ROLL]   = 0;
-    init_cmd[COMMAND_PITCH]  = 0;
-    init_cmd[COMMAND_YAW]    = 0;
-    supervision_run(TRUE, FALSE, init_cmd);
-  }
-
   /* scale final motor commands to 0-1 for feeding the fdm */
   /* FIXME: autopilot.commands is of length NB_COMMANDS instead of number of motors */
   for (uint8_t i=0; i<SUPERVISION_NB_MOTOR; i++)

sw/simulator/nps/nps_fdm.h

@@ -40,6 +40,8 @@
 struct NpsFdm {
 
   double time;
+  double init_dt;
+  double curr_dt;
   bool_t on_ground;
 
   /*  position */

sw/simulator/nps/nps_fdm_jsbsim.c

@@ -43,6 +43,11 @@
 /// Macro to convert from feet to metres
 #define MetersOfFeet(_f) ((_f)/3.2808399)
 
+/** Minimum JSBSim timestep
+  * Around 1/10000 seems to be good for ground impacts
+  */
+#define MIN_DT (1.0/10240.0)
+
 using namespace JSBSim;
 
 static void feed_jsbsim(double* commands);
@@ -68,14 +73,30 @@ static FGFDMExec* FDMExec;
 
 static struct LtpDef_d ltpdef;
 
+/// The largest distance between vehicle CG and contact point
+double vehicle_radius_max;
+
+/// Timestep used for higher fidelity near the ground
+double min_dt;
+
 void nps_fdm_init(double dt) {
 
+  fdm.init_dt = dt;
+  fdm.curr_dt = dt;
+  //Sets up the high fidelity timestep as a multiple of the normal timestep
+  for (min_dt = (1.0/dt); min_dt < (1/MIN_DT); min_dt += (1/dt)){}
+  min_dt = (1/min_dt);
+
   init_jsbsim(dt);
 
   FDMExec->RunIC();
 
   init_ltp();
 
+#if DEBUG_NPS_JSBSIM
+  printf("fdm.time,fg_body_ecef_accel1,fg_body_ecef_accel2,fg_body_ecef_accel3,fdm.body_ecef_accel.x,fdm.body_ecef_accel.y,fdm.body_ecef_accel.z,fg_ltp_ecef_accel1,fg_ltp_ecef_accel2,fg_ltp_ecef_accel3,fdm.ltp_ecef_accel.x,fdm.ltp_ecef_accel.y,fdm.ltp_ecef_accel.z,fg_ecef_ecef_accel1,fg_ecef_ecef_accel2,fg_ecef_ecef_accel3,fdm.ecef_ecef_accel.x,fdm.ecef_ecef_accel.y,fdm.ecef_ecef_accel.z,fdm.ltpprz_ecef_accel.z,fdm.ltpprz_ecef_accel.y,fdm.ltpprz_ecef_accel.z,fdm.agl\n");
+#endif
+
   fetch_state();
 
 }
@@ -84,7 +105,45 @@ void nps_fdm_run_step(double* commands) {
 
   feed_jsbsim(commands);
 
-  FDMExec->Run();
+  /* To deal with ground interaction issues, we decrease the time
+     step as the vehicle is close to the ground. This is done predictively
+     to ensure no weird accelerations or oscillations. From tests with a bouncing
+     ball model in JSBSim, it seems that 10k steps per second is reasonable to capture
+     all the dynamics. Higher might be a bit more stable, but really starting to push
+     the simulation CPU requirements, especially for more complex models.
+      - at init: get the largest radius from CG to any contact point (landing gear)
+      - if descending...
+        - find current number of timesteps to impact
+        - if impact imminent, calculate a new timestep to use (with limit)
+      - if ascending...
+        - change timestep back to init value
+      - run sim for as many steps as needed to reach init_dt amount of time
+
+     Of course, could probably be improved...
+  */
+  // If the vehicle has a downwards velocity
+  if (fdm.ltp_ecef_vel.z > 0) {
+    // Get the current number of timesteps until impact at current velocity
+    double numDT_to_impact = (fdm.agl - vehicle_radius_max) / (fdm.curr_dt * fdm.ltp_ecef_vel.z);
+    // If impact imminent within next timestep, use high sim rate
+    if (numDT_to_impact <= 1.0) {
+      fdm.curr_dt = min_dt;
+    }
+  }
+  // If the vehicle is moving upwards and out of the ground, reset timestep
+  else if ((fdm.ltp_ecef_vel.z <= 0) && ((fdm.agl + vehicle_radius_max) > 0)) {
+    fdm.curr_dt = fdm.init_dt;
+  }
+
+  // Calculate the number of sim steps for correct amount of time elapsed
+  int num_steps = int(fdm.init_dt / fdm.curr_dt);
+
+  // Set the timestep then run sim
+  FDMExec->Setdt(fdm.curr_dt);
+  int i;
+  for (i = 0; i < num_steps; i++) {
+    FDMExec->Run();
+  }
 
   fetch_state();
 
@@ -118,6 +177,10 @@ static void fetch_state(void) {
   FGPropertyManager* node = FDMExec->GetPropertyManager()->GetNode("simulation/sim-time-sec");
   fdm.time = node->getDoubleValue();
 
+#if DEBUG_NPS_JSBSIM
+  printf("%f,",fdm.time);
+#endif
+
   FGPropagate* propagate = FDMExec->GetPropagate();
   FGAccelerations* accelerations = FDMExec->GetAccelerations();
 
@@ -139,25 +202,41 @@ static void fetch_state(void) {
   const FGColumnVector3& fg_body_ecef_accel = accelerations->GetUVWdot();
   jsbsimvec_to_vec(&fdm.body_ecef_accel,&fg_body_ecef_accel);
 
+#if DEBUG_NPS_JSBSIM
+  printf("%f,%f,%f,%f,%f,%f,",(&fg_body_ecef_accel)->Entry(1),(&fg_body_ecef_accel)->Entry(2),(&fg_body_ecef_accel)->Entry(3),fdm.body_ecef_accel.x,fdm.body_ecef_accel.y,fdm.body_ecef_accel.z);
+#endif
+
   /* in LTP frame */
   const FGMatrix33& body_to_ltp = propagate->GetTb2l();
   const FGColumnVector3& fg_ltp_ecef_vel = body_to_ltp * fg_body_ecef_vel;
   jsbsimvec_to_vec((DoubleVect3*)&fdm.ltp_ecef_vel, &fg_ltp_ecef_vel);
   const FGColumnVector3& fg_ltp_ecef_accel = body_to_ltp * fg_body_ecef_accel;
   jsbsimvec_to_vec((DoubleVect3*)&fdm.ltp_ecef_accel, &fg_ltp_ecef_accel);
 
+#if DEBUG_NPS_JSBSIM
+  printf("%f,%f,%f,%f,%f,%f,",(&fg_ltp_ecef_accel)->Entry(1),(&fg_ltp_ecef_accel)->Entry(2),(&fg_ltp_ecef_accel)->Entry(3),fdm.ltp_ecef_accel.x,fdm.ltp_ecef_accel.y,fdm.ltp_ecef_accel.z);
+#endif
+
   /* in ECEF frame */
   const FGMatrix33& body_to_ecef = propagate->GetTb2ec();
   const FGColumnVector3& fg_ecef_ecef_vel = body_to_ecef * fg_body_ecef_vel;
   jsbsimvec_to_vec((DoubleVect3*)&fdm.ecef_ecef_vel, &fg_ecef_ecef_vel);
   const FGColumnVector3& fg_ecef_ecef_accel = body_to_ecef * fg_body_ecef_accel;
   jsbsimvec_to_vec((DoubleVect3*)&fdm.ecef_ecef_accel, &fg_ecef_ecef_accel);
 
+#if DEBUG_NPS_JSBSIM
+  printf("%f,%f,%f,%f,%f,%f,",(&fg_ecef_ecef_accel)->Entry(1),(&fg_ecef_ecef_accel)->Entry(2),(&fg_ecef_ecef_accel)->Entry(3),fdm.ecef_ecef_accel.x,fdm.ecef_ecef_accel.y,fdm.ecef_ecef_accel.z);
+#endif
+
   /* in LTP pprz */
   ned_of_ecef_point_d(&fdm.ltpprz_pos, &ltpdef, &fdm.ecef_pos);
   ned_of_ecef_vect_d(&fdm.ltpprz_ecef_vel, &ltpdef, &fdm.ecef_ecef_vel);
   ned_of_ecef_vect_d(&fdm.ltpprz_ecef_accel, &ltpdef, &fdm.ecef_ecef_accel);
 
+#if DEBUG_NPS_JSBSIM
+  printf("%f,%f,%f,",fdm.ltpprz_ecef_accel.z,fdm.ltpprz_ecef_accel.y,fdm.ltpprz_ecef_accel.z);
+#endif
+
   /* llh */
   llh_from_jsbsim(&fdm.lla_pos, propagate);
 
@@ -167,6 +246,9 @@ static void fetch_state(void) {
   lla_of_ecef_d(&fdm.lla_pos_pprz, &fdm.ecef_pos);
   fdm.agl = MetersOfFeet(propagate->GetDistanceAGL());
 
+#if DEBUG_NPS_JSBSIM
+  printf("%f\n",fdm.agl);
+#endif
 
   /*
    * attitude
@@ -237,6 +319,16 @@ static void init_jsbsim(double dt) {
     exit(-1);
   }
 
+  // calculate vehicle max radius in m
+  vehicle_radius_max = 0.01; // specify not 0.0 in case no gear
+  int num_gear = FDMExec->GetGroundReactions()->GetNumGearUnits();
+  int i;
+  for(i = 0; i < num_gear; i++) {
+    FGColumnVector3 gear_location = FDMExec->GetGroundReactions()->GetGearUnit(i)->GetBodyLocation();
+    double radius = MetersOfFeet(gear_location.Magnitude());
+    if (radius > vehicle_radius_max) vehicle_radius_max = radius;
+  }
+
 }
 
 /**

sw/simulator/nps/nps_flightgear.c

@@ -52,9 +52,8 @@ void nps_flightgear_send() {
   gui.num_tanks = 1;
   gui.fuel_quantity[0] = 0.;
 
-  //  gui.cur_time = 3198060679ul;
-  gui.cur_time = 3198060679ul + rint(fdm.time);
-  ///  gui.cur_time = 3198101679ul;
+  //gui.cur_time = 3198060679ul + rint(fdm.time);
+  gui.cur_time = 3198101679ul + rint(fdm.time);
   gui.warp = 1122474394ul;
 
   gui.ground_elev = 0.;

sw/simulator/nps/nps_main.c

@@ -13,12 +13,14 @@
 #include "nps_ivy.h"
 #include "nps_flightgear.h"
 
-#define SIM_DT     (1./512.)
+#include "mcu_periph/sys_time.h"
+#define SIM_DT     (SYS_TIME_RESOLUTION)
 #define DISPLAY_DT (1./30.)
 #define HOST_TIMEOUT_MS 40
 #define HOST_TIME_FACTOR 1.
 
 static struct {
+  double real_initial_time;
   double scaled_initial_time;
   double host_time_factor;
   double sim_time;
@@ -82,6 +84,7 @@ static void nps_main_init(void) {
   nps_main.display_time = 0.;
   struct timeval t;
   gettimeofday (&t, NULL);
+  nps_main.real_initial_time = time_to_double(&t);
   nps_main.scaled_initial_time = time_to_double(&t);
   nps_main.host_time_factor = HOST_TIME_FACTOR;
 
@@ -107,6 +110,10 @@ static void nps_main_init(void) {
   if (nps_main.fg_host)
     nps_flightgear_init(nps_main.fg_host, nps_main.fg_port);
 
+#if DEBUG_NPS_TIME
+  printf("host_time_factor,host_time_elapsed,host_time_now,scaled_initial_time,sim_time_before,display_time_before,sim_time_after,display_time_after\n");
+#endif
+
 }
 
 
@@ -160,6 +167,7 @@ static gboolean nps_main_periodic(gpointer data __attribute__ ((unused))) {
     t2 = time_to_double(&tv_now);
     /* add the pause to initial real time */
     irt += t2 - t1;
+    nps_main.real_initial_time += t2 - t1;
     /* convert to scaled initial real time */
     nps_main.scaled_initial_time = t2 - (t2 - irt)/nps_main.host_time_factor;
     pauseSignal = 0;
@@ -169,15 +177,37 @@ static gboolean nps_main_periodic(gpointer data __attribute__ ((unused))) {
   host_time_now = time_to_double(&tv_now);
   double host_time_elapsed = nps_main.host_time_factor *(host_time_now  - nps_main.scaled_initial_time);
 
+#if DEBUG_NPS_TIME
+  printf("%f,%f,%f,%f,%f,%f,",nps_main.host_time_factor,host_time_elapsed,host_time_now,nps_main.scaled_initial_time,nps_main.sim_time,nps_main.display_time);
+#endif
+
+  int cnt = 0;
+  static int prev_cnt = 0;
+  static int grow_cnt = 0;
   while (nps_main.sim_time <= host_time_elapsed) {
     nps_main_run_sim_step();
     nps_main.sim_time += SIM_DT;
-    if (nps_main.display_time < nps_main.sim_time) {
+    if (nps_main.display_time < (host_time_now - nps_main.real_initial_time)) {
       nps_main_display();
       nps_main.display_time += DISPLAY_DT;
     }
+    cnt++;
+  }
+
+  /* Check to make sure the simulation doesn't get too far behind real time looping */
+  if (cnt > (prev_cnt)) {grow_cnt++;}
+  else { grow_cnt--;}
+  if (grow_cnt < 0) {grow_cnt = 0;}
+  prev_cnt = cnt;
+
+  if (grow_cnt > 10) {
+    printf("Warning: The time factor is too large for efficient operation! Please reduce the time factor.\n");
   }
 
+#if DEBUG_NPS_TIME
+  printf("%f,%f\n",nps_main.sim_time,nps_main.display_time);
+#endif
+
   return TRUE;
 
 }
@@ -194,11 +224,12 @@ static bool_t nps_main_parse_options(int argc, char** argv) {
   static const char* usage =
 "Usage: %s [options]\n"
 " Options :\n"
-"   --fg_host flight gear host\n"
-"   --fg_port flight gear port\n"
-"   -j --js_dev joystick device\n"
-"   --spektrum_dev spektrum device\n"
-"   --rc_script no\n";
+"   -h                                     Display this help\n"
+"   --fg_host <flight gear host>           e.g. 127.0.0.1\n"
+"   --fg_port <flight gear port>           e.g. 5501\n"
+"   -j --js_dev <joystick device or index> e.g. /dev/input/js0 or 0\n"
+"   --spektrum_dev <spektrum device>       e.g. /dev/ttyUSB0\n"
+"   --rc_script <number>                   e.g. 0\n";
 
 
   while (1) {
@@ -212,7 +243,7 @@ static bool_t nps_main_parse_options(int argc, char** argv) {
       {0, 0, 0, 0}
     };
     int option_index = 0;
-    int c = getopt_long(argc, argv, "j:",
+    int c = getopt_long(argc, argv, "j:h",
                         long_options, &option_index);
     if (c == -1)
       break;
@@ -237,6 +268,10 @@ static bool_t nps_main_parse_options(int argc, char** argv) {
       nps_main.js_dev = strdup(optarg);
       break;
 
+    case 'h':
+      fprintf(stderr, usage, argv[0]);
+      exit(0);
+
     default: /* $B!G(B?$B!G(B */
       printf("?? getopt returned character code 0%o ??\n", c);
       fprintf(stderr, usage, argv[0]);

sw/simulator/nps/nps_radio_control.c

@@ -76,18 +76,21 @@ static rc_script scripts[] = {
 bool_t nps_radio_control_available(double time) {
   if (time >=  nps_radio_control.next_update) {
     nps_radio_control.next_update += RADIO_CONTROL_DT;
-    if (time < RADIO_CONTROL_TAKEOFF_TIME)
-      radio_control_script_takeoff(time);
-    else if (nps_radio_control.type == SCRIPT)
-      scripts[nps_radio_control.num_script](time);
-    else if (nps_radio_control.type == JOYSTICK) {
+
+    if (nps_radio_control.type == JOYSTICK) {
       nps_radio_control.throttle = nps_joystick.throttle;
       nps_radio_control.roll = nps_joystick.roll;
       nps_radio_control.pitch = nps_joystick.pitch;
       nps_radio_control.yaw = nps_joystick.yaw;
       nps_radio_control.mode = nps_joystick.mode;
       //printf("throttle: %f, roll: %f, pitch: %f, yaw: %f\n", nps_joystick.throttle, nps_joystick.roll, nps_joystick.pitch, nps_joystick.yaw);
-    }
+    } else
+      if (nps_radio_control.type == SCRIPT) {
+        if (time < RADIO_CONTROL_TAKEOFF_TIME)
+          radio_control_script_takeoff(time);
+        else
+          scripts[nps_radio_control.num_script](time);
+      }
     return TRUE;
   }
   return FALSE;