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 */
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;
}
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 */
/**
@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);
}
/**
@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);
}
}
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 */