updated timing to use autopilot PERIODIC_FREQUENCY as base, jsbsim st…

…eps based on SIM_DT, fixed out of ground timestep check to include vehicle radius, added some debug printf's based on DEBUG_NPS_JSBSIM and DEBUG_NPS_TIME, checks if simulation runs will get behind real time at higher time factors, tried to fix display output timing, still needs to be fixed

sw/simulator/nps/nps_fdm_jsbsim.c

@@ -43,7 +43,9 @@
 /// Macro to convert from feet to metres
 #define MetersOfFeet(_f) ((_f)/3.2808399)
 
-/// Minimum JSBSim timestep
+/** Minimum JSBSim timestep
+  * Around 1/10000 seems to be good for ground impacts
+  */
 #define MIN_DT (1.0/10240.0)
 
 using namespace JSBSim;
@@ -74,16 +76,27 @@ 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();
 
 }
@@ -114,11 +127,11 @@ void nps_fdm_run_step(double* commands) {
     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;
+      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 > 0)) {
+  else if ((fdm.ltp_ecef_vel.z <= 0) && ((fdm.agl + vehicle_radius_max) > 0)) {
     fdm.curr_dt = fdm.init_dt;
   }
 
@@ -164,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();
 
@@ -185,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);
 
@@ -213,6 +246,10 @@ 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
    */

sw/simulator/nps/nps_main.c

@@ -13,12 +13,17 @@
 #include "nps_ivy.h"
 #include "nps_flightgear.h"
 
-#define SIM_DT     (1./512.)
+#if SYS_TIME_RESOLUTION
+#define SIM_DT     (SYS_TIME_RESOLUTION)
+#else
+#define SIM_DT     (1./(double)(PERIODIC_FREQUENCY))
+#endif /* 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 +87,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 +113,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
+
 }
 
 
@@ -169,15 +179,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;
 
 }