Merge pull request #61 from nasa-jpl/bowkett-prof-vel-init-tweak

Add minimum dt to trap_update_vel

Author: JosephBowkett

src/trap.c

@@ -488,7 +488,7 @@ int trap_update_vel(trap_t* self, double t, double* pos, double* vel)
   double dt;
 
   if (t < self->t_acc) {
-    dt   = t - self->t_init;
+    dt   = fmax(t - self->t_init, 0.001);
     *vel = self->vel_init + self->acc * dt;
     *pos = self->pos_init + self->vel_init * dt + 0.5 * self->acc * dt * dt;
   } else {

test/test_unit/CMakeLists.txt

@@ -10,6 +10,8 @@ set(TEST_SOURCES
     test_linear_interpolation.cc
 
     test_jsd_device_base.cc
+
+    test_trap.cc
     )
 
 foreach(TEST_SOURCE ${TEST_SOURCES})

test/test_unit/test_trap.cc

@@ -0,0 +1,63 @@
+#include <gtest/gtest.h>
+
+#include "fastcat/trap.h"
+
+namespace
+{
+class TrapTest : public ::testing::Test
+{
+ protected:
+  void SetUp() override {
+  }
+
+  trap_t trap_1_;
+};
+
+TEST_F(TrapTest, TrapVelocityReInit){
+
+  double time_initial = 0.0;
+  double position_initial = 0.0;
+  double velocity_initial = 0.0;
+  double velocity_final = 1.0;
+  double acceleration = 1.0;
+  double max_time = 10.0;
+
+  // Generate the first version of the trap
+  trap_generate_vel(&trap_1_,
+                    time_initial,
+                    position_initial,
+                    velocity_initial,
+                    velocity_final,
+                    acceleration,
+                    max_time);
+
+  double dt = 0.01;
+  
+  double tracking_time = time_initial;
+  double tracking_position = position_initial;
+  double tracking_velocity = velocity_initial;
+
+  for (int i=0; i<1000; i++) {
+    // Update trap before any change in time has occurred
+    trap_update_vel(&trap_1_, tracking_time,
+                    &tracking_position,
+                    &tracking_velocity);
+
+    // Increment time
+    tracking_time += dt;
+    
+    // Regenerate trap to simulate incoming updated setpoint
+    trap_generate_vel(&trap_1_, tracking_time,
+                      tracking_position,
+                      tracking_velocity,
+                      velocity_final,
+                      acceleration,
+                      max_time);
+  }
+
+  // The minimum dt used in track vel should guarantee that
+  // motion occurs despite regeneration happening every cycle
+  EXPECT_TRUE(tracking_position > position_initial);
+}
+
+}