Commits

Anonymous committed 68fd7d4

Moved control input clamp from wing kinematics function to control input function.
Added approximate step function.

  • Participants
  • Parent commits ea3bb03

Comments (0)

Files changed (2)

 {
   int rtn_flag = SUCCESS;
   float deriv_ctlr_err;
+  float max_u;
 
   // Get controller error based on controller type
   ctlr_err[0] = ctlr_err[1];
   *u = config.ctlr_param.pgain*ctlr_err[1] + config.ctlr_param.dgain*deriv_ctlr_err;
   *u = *u*RAD2DEG;
 
+  // Clamp control signal
+  switch (config.kine_param.type) {
+
+      case DIFF_AOA_ID:
+          max_u = DIFF_AOA_MAX_U;
+          break;
+
+      case DIFF_DEV_ID:
+          max_u = DIFF_DEV_MAX_U;
+          break;
+
+      default:
+          PRINT_ERR_MSG("unknown kinematics type");
+          break;
+  }
+
+  *u = *u < (float) max_u ? *u : (float) max_u;
+  *u = *u > -(float) max_u ? *u : -(float)max_u;
+
   return rtn_flag;
 }
 
   float rot_amp;
   float str_k;
   float rot_k;
-  float u_clamped;
+  //float u_clamped;
 
   // Get kinematics parameters from configuration
   T = config.kine_param.period;
 
     case DIFF_AOA_ID:
 
-      // Clamp control signal
-      u_clamped = u < (float) DIFF_AOA_MAX_U ? u : (float) DIFF_AOA_MAX_U;
-      u_clamped = u_clamped > -(float)DIFF_AOA_MAX_U ? u_clamped : -(float)DIFF_AOA_MAX_U;
-      
       vals[STROKE_0_ID] = (str_amp/asinf(str_k))*asinf(str_k*((float)cos(2.0*M_PI*t/(double)T)));
       vals[STROKE_1_ID] = (str_amp/asinf(str_k))*asinf(str_k*((float)cos(2.0*M_PI*t/(double)T)));
-      vals[ROTATION_0_ID] = (rot_amp/tanhf(rot_k))*tanhf(rot_k*((float)sin(2.0*M_PI*t/(double)T))) - u_clamped;
-      vals[ROTATION_1_ID] = (rot_amp/tanhf(rot_k))*tanhf(rot_k*((float)sin(2.0*M_PI*t/(double)T))) + u_clamped;
+      vals[ROTATION_0_ID] = (rot_amp/tanhf(rot_k))*tanhf(rot_k*((float)sin(2.0*M_PI*t/(double)T))) - u;
+      vals[ROTATION_1_ID] = (rot_amp/tanhf(rot_k))*tanhf(rot_k*((float)sin(2.0*M_PI*t/(double)T))) + u;
       vals[DEVIATION_0_ID] = 0.0;
       vals[DEVIATION_1_ID] = 0.0;
      break;
 
     case DIFF_DEV_ID:
 
-      // Clamp control signal
-      u_clamped = u < DIFF_DEV_MAX_U ? u : DIFF_DEV_MAX_U;
-      u_clamped = u_clamped > -DIFF_DEV_MAX_U ? u : -DIFF_DEV_MAX_U;
-
       vals[STROKE_0_ID] = (str_amp/asinf(str_k))*asinf(str_k*((float)cos(2.0*M_PI*t/(double)T)));
       vals[STROKE_1_ID] = (str_amp/asinf(str_k))*asinf(str_k*((float)cos(2.0*M_PI*t/(double)T)));
       vals[ROTATION_0_ID] = (rot_amp/tanhf(rot_k))*tanhf(rot_k*((float)sin(2.0*M_PI*t/(double)T)));
       vals[ROTATION_1_ID] = (rot_amp/tanhf(rot_k))*tanhf(rot_k*((float)sin(2.0*M_PI*t/(double)T)));
-      vals[DEVIATION_0_ID] = u_clamped;
-      vals[DEVIATION_1_ID] = -u_clamped; 
+      vals[DEVIATION_0_ID] =  u;
+      vals[DEVIATION_1_ID] = -u; 
      break; 
 
     default:
 
   ///////////////////////////////////////////////////////////////////
   // Constant torque test  - set torque to some know value.
-  // torq_raw = 0.01;
+  // torq_raw = 0.015;
+  // torq_raw = torq_raw + 0.015;
   ///////////////////////////////////////////////////////////////////
 
   // Apply deadband about zero to limit drift

python/libyawff/yawff_utils.py

         else:
             return 0
 
+def approx_step_func(t,t0,u0,delta_t):
+    """
+    Approximate step function. Transitions from 0 to u0 w/ transition midpoint
+    t0. The transition is linear starting at t0-delta_t and ending at
+    t0+delta_t.
+
+    Inputs:
+      t       = time points
+      t0      = midpoint of step transition
+      u0      = step height
+      delta_t = transition width.
+
+    Returns: array of function values evaluated at the points in t 
+    """
+
+    t_orig_shape = t.shape
+    if len(t.shape) == 2:
+        assert ((t.shape[0] == 1) or (t.shape[1] == 1)), 'array shape must be (n,), (n,1) or (1,n)'
+        if t.shape[1] == 1:
+            tt = scipy.reshape(t,(t.shape[0],))
+        else:
+            tt =scipy.reshape(t,(t.shape[1],))
+    else:
+        tt = t
+
+    mask0 = tt<(t0-0.5*delta_t)
+    mask1 = scipy.logical_and(tt>=(t0-0.5*delta_t), tt<=(t0+0.5*delta_t))
+    mask2 = tt>(t0+0.5*delta_t)
+    vals = scipy.zeros(tt.shape)
+
+    vals[mask0] = 0.0
+    vals[mask1] = (u0/delta_t)*tt[mask1] - (u0/delta_t)*(t0 - 0.5*delta_t)
+    vals[mask2] = u0
+    vals = scipy.reshape(vals,t_orig_shape)
+    return vals
+
 def ramp_to_const_vel(t,vel,accel):
     """
     Generates a ramp trajectory to constant velocity.