void ST_setupVelPlan(ST_Handle handle) {

  _iq accMax, jrkMax;
  ST_Obj *stObj = (ST_Obj *)handle;

  // Pass the configuration array pointer into SpinTAC Velocity Plan
  STVELPLAN_setCfgArray(stObj->velPlanHandle, &stVelPlanCfgArray[0], sizeof(stVelPlanCfgArray), 0, 0, 0, NUM_PLAN_TRANS, NUM_PLAN_STATES);

  // Establish the Acceleration, and Jerk Maximums
  accMax = _IQ24(10.0);
  jrkMax = _IQ20(62.5);

  // Configure SpinTAC Velocity Plan: Sample Time, LoopENB
  STVELPLAN_setCfg(stObj->velPlanHandle, _IQ(ST_SAMPLE_TIME), false);
  // Configure halt state: VelEnd, AccMax, JrkMax, Timer
  STVELPLAN_setCfgHaltState(stObj->velPlanHandle, 0, accMax, jrkMax, 1000L);

  //Example: STVELPLAN_addCfgState(handle,    VelSetpoint[pups],                 StateTimer[ticks]);
  STVELPLAN_addCfgState(stObj->velPlanHandle, 0,                                 200L); // StateA
  STVELPLAN_addCfgState(stObj->velPlanHandle, _IQ(0.25 * ST_SPEED_PU_PER_KRPM),  2000L); // StateB
  STVELPLAN_addCfgState(stObj->velPlanHandle, _IQ(-0.25 * ST_SPEED_PU_PER_KRPM), 2000L); // StateC

  //Example: STVELPLAN_addCfgTran(handle,    FromState, ToState, CondOption, CondIdx1, CondiIdx2, AccLim[pups2], JrkLim[pups3]);
  STVELPLAN_addCfgTran(stObj->velPlanHandle, STATE_A,   STATE_B, ST_COND_NC, 0,        0,         _IQ(0.1),      _IQ20(1));	// From StateA to StateB
  STVELPLAN_addCfgTran(stObj->velPlanHandle, STATE_B,   STATE_C, ST_COND_NC, 0,        0,         _IQ(0.1),      _IQ20(1));	// From StateB to StateC
  STVELPLAN_addCfgTran(stObj->velPlanHandle, STATE_C,   STATE_A, ST_COND_NC, 0,        0,         _IQ(1),        _IQ20(1));	// From StateC to StateA
  // Note: set CondIdx1 to 0 if CondOption is ST_COND_NC; set CondIdx2 to 0 if CondOption is ST_COND_NC  or ST_COND_FC
}
TEST(IQmath_ExtractInteger, IQ24int)
{
    LONGS_EQUAL(127, _IQ24int(_IQ24(127.999999940))) ;
    LONGS_EQUAL(-128, _IQ24int(_IQ24(-128.0))) ;
}