コード例 #1
0
extern fractional* BartlettInit (	/* Initialize a Bartlett window */
					/* computed in floating point */
					/* converted to fractionals */
   int numElems,			/* number elements in window */
   fractional* window			/* ptr to window */
					/* window returned */
) {

   /* Local declarations. */
   fractional* retWindow = window;
   double arg = 2.0/((double) (numElems-1));
   int cntr = 0;

   /* Compute window factors. */
   for (cntr = 0; cntr <= (numElems-1)/2; cntr++) {
      *(window++) = Float2Fract (arg*cntr);
   }
   for ( ; cntr < numElems; cntr++) {
      *(window++) = Float2Fract (BART_0 - arg*cntr);
   }

   /* Return window pointer. */
   return (retWindow);

} /* end of BartlettInit */
コード例 #2
0
void FC_Init()
{
    
    // Roll PID setup
    FC_Set_Roll_Reference(0);
    FC_PID_Set_Roll_Gains(&PIDRoll, Float2Fract(ROLL_Kp), Float2Fract(ROLL_Ki), Float2Fract(ROLL_Kd));

    // Pitch PID setup
    FC_Set_Pitch_Reference(0);
    FC_PID_Set_Pitch_Gains(&PIDPitch, Float2Fract(PITCH_Kp), Float2Fract(PITCH_Ki), Float2Fract(PITCH_Kd));

    // Yaw PID setup
    FC_Set_Yaw_Reference(0);
    FC_PID_Set_Yaw_Gains(&PIDYaw, Float2Fract(YAW_Kp), Float2Fract(YAW_Ki), Float2Fract(YAW_Kd));
    
    FC_Init_Filters();

    // Begin control timer
    FCStatus = 0;
    prev_error_yaw = 0;
    FC_Timer_Init();

    FCGyroIntegral.Gy_X = 0.0;
    FCGyroIntegral.Gy_Y = 0.0;
    FCGyroIntegral.Gy_Z = 0.0;

}
コード例 #3
0
extern fractcomplex* CosFactorInit (	/* Initialize cosine factors */
					/* CN(k) = exp(i*k*pi/(2*N)) */
					/* computed in floating point */
					/* converted to fractionals */
   int log2N,				/* log2(N), N real plus imag factors */
   					/* (only N/2 complex factors) */
   					/* NOTE: only half of cosine factors */
					/* are used for DCT computation */
   fractcomplex* cosineFactors		/* ptr to cosine factors */
					/* cosineFactors returned */
					/* only the first half: */
					/* CN(0)...CN(N/2-1) */
) {

   /* Local declarations. */
   fractcomplex* retFactors = cosineFactors;
   /* NOTE: instead of cReal, cImag, cRecReal and cRecImag declared	*/
   /* as double, it would have been cleaner to declare c and cRec as	*/
   /* fractcomplex. In this case, the values c.real, c.imag cRec.real	*/
   /* and cRec.imag would have replaced the instances of cReal, cImag,	*/
   /* cRecReal and cRecImag, respectively. However, if declared as	*/
   /* fractcomplex structures, the PIC30 simulator fails to compute	*/
   /* the cosine factors even though the results are correct when	*/
   /* compiling solely under CYGWIN!!! Hence, the variables have	*/
   /* been declared individual doubles.					*/
   double cReal = 0.0;
   double cImag = 0.0;
   double cRecReal = 0.0;
   double cRecImag = 0.0;
   double cTmp = 0.0;
   double arg = PI;				/* sin/cos argument */
   						/* (default PI) */
   int numFactors = (1<<log2N);			/* cosine plus sine factors */
   int cntr = 0;

   /* Set up cosine-sine factor computation. */
   arg /= (2.0*numFactors);			/* PI/(2*N) */
   cRecReal = cos(arg);
   cRecImag = -sin(arg);
   cReal = 1.0;
   cImag = 0.0;

   /* Compute cosine-sine factors recursively. */
   for (cntr = numFactors/2-1; cntr >= 0; cntr--) {
      cosineFactors->real = Float2Fract (cReal);
      (cosineFactors++)->imag = Float2Fract (cImag);
      cTmp = cReal;
      cReal = cReal*cRecReal + cImag*cRecImag;
      cImag = cImag*cRecReal - cTmp*cRecImag;
   }

   /* Return destination vector pointer. */
   return (retFactors);

} /* end of CosFactorInit */
コード例 #4
0
ファイル: current.c プロジェクト: NxRLab/gibbot
/**
    @param alpha The EMA coefficient, 0 <= alpha <= 1.
    An exponential moving average is applied to the offset in #Current.offset
    whenever the motor pins are floating.  The equation is defined recursively:

        EMA(i) = alpha * x + (1-alpha) * EMA(i-1).

    The weighting coefficient alpha is by default #EMA_ALPHA at startup.

    @warning values exceeding the range for alpha are clamped at 0 or 1,
    whichever number is closest.
*/
void set_offset_ema_filter_alpha(double alpha) 
{ 
    if(alpha < 0) {
        alpha = 0;
    }
    else if(alpha > 1) {
        alpha = 1;
    }

    offset_ema_filter[0] = Float2Fract(alpha);
    offset_ema_filter[1] = Float2Fract(1.0 - alpha);
}
コード例 #5
0
ファイル: current.c プロジェクト: NxRLab/gibbot
/**
    @param arr an array of #ADC_SAMPS entires or NULL if an averaging filter is
    fine.

    @note The values of arr will be truncated to a Q15 fixed point number.  This
    may lead to loss of precision of the filter coefficients.  If this is a
    concern then the user should only store values that can be represented by a
    Q15 number.  Useful functions provided by Microchip's DSP library in dsp.h
    are Fract2Float and Float2Fract, which takes in a 32-bit \c float / \c
    double (by default they're the same in xc16 v1.24) or a \c fractional type
    depending on which way the conversion is being done.

    @warning The results of applying the new filter may not appear immediately.
    It may take a few DMA transfers to see correct operation.  It is up to the
    user to determine when this has occurred.  A simple solution is to call one
    of Microchip's __delay functions after calling this function.  This should
    not be necessary though.
*/
void set_cur_filter_coeffs(double *arr) 
{ 
    int i; 
    double c;

    c = 1.0 / ADC_SAMPS;
    for(i = 0; i < ADC_SAMPS; i++) {
        c = (arr == NULL) ? c : arr[i];
        cur_filter[i] = Float2Fract(c);
    }
}
コード例 #6
0
extern fractional* HanningInit (	/* Initialize a Hanning window */
					/* computed in floating point */
					/* converted to fractionals */
   int numElems,			/* number elements in window */
   fractional* window			/* ptr to window */
					/* window returned */
) {

   /* Local declarations. */
   fractional* retWindow = window;
   double arg = 2.0*PI/((double) (numElems-1));
   int cntr = 0;

   /* Compute window factors. */
   for (cntr = 0; cntr < numElems; cntr++) {
      *(window++) = Float2Fract (HANN_0 + HANN_1*cos(arg*cntr));
   }

   /* Return window pointer. */
   return (retWindow);

} /* end of HanningInit */