void _lowpass_filter_step(uint32_t n_dims, value_t *input,
value_t *output, void *pars)
{
// Cast the params
lowpass_state_t *params = (lowpass_state_t *) pars;
register int32_t a = bitsk(params->a);
register int32_t b = bitsk(params->b);
// Apply the filter to every dimension (realised as a Direct Form I digital
// filter).
for (uint32_t d = 0; d < n_dims; d++)
{
// The following is equivalent to:
//
// output[d] *= params->a;
// output[d] += input[d] * params->b;
// Compute the next value in a register
register int64_t next_output;
// Perform the first multiply
int32_t current_output = bitsk(output[d]);
next_output = __smull(current_output, a);
// Perform the multiply accumulate
int32_t current_input = bitsk(input[d]);
next_output = __smlal(next_output, current_input, b);
// Scale the result back down to store it
output[d] = kbits(convert_s32_30_s16_15(next_output));
}
}
void DoFakeCalibSequence(_Fract* yRateBias, volatile _Fract* yRateNoise,
_Fract* xAxisBias, volatile _Fract* xAxisNoise,
_Fract* zAxisBias, volatile _Fract* zAxisNoise,
_Fract* gravity, _Accum* gyroGain)
{
int16_t yRateBiasInt = FakeYrateBias;
int16_t yRateNoiseInt = FakeYrateNoise;
// Fake condtions
*yRateBias = rbits(yRateBiasInt);
*xAxisBias = rbits(FakeXaxisBias);
*zAxisBias = rbits(FakeZaxisBias);
*gravity = rbits(FakeGravity);
gSquared = (*gravity)*(*gravity);
*gyroGain = kbits(FakeGyroGain);
*yRateNoise = rbits(yRateNoiseInt);
*xAxisNoise = rbits(FakeXaxisNoise);
*zAxisNoise = rbits(FakeZaxisNoise);
#ifdef ForceGyroCalib
waitUntilFacing(CircuitSideUp, 0, 0, 0);
_Accum gyroIntegral = 0;
waitUntilFacing(CircuitSideDown, &upYAverage, &upYNoise, &gyroIntegral);
// gyro integral should correspond to a 180 degree flip.
// 180 deg = 1 in binary angle
*gyroGain = (1.0/gyroIntegral);
#endif
}
void _lti_filter_step(uint32_t n_dims, value_t *input,
value_t *output, void *s)
{
// Cast the state
lti_state_t *state = (lti_state_t *) s;
// Apply the filter to every dimension (realised as a Direct Form I digital
// filter).
for (uint32_t d = n_dims, dd = n_dims - 1; d > 0; d--, dd--)
{
// Point to the correct previous x and y values.
ab_t *xy = &state->xyz[dd * state->order];
// Create the new output value for this dimension
register int64_t output_val = 0;
// Direct Form I filter
// `m` acts as an index into the ring buffer of historic input and output.
for (uint32_t k=0, m = state->n; k < state->order; k++)
{
// Update the index into the ring buffer, if this would go negative it
// wraps to the top of the buffer.
if (m == 0)
{
m += state->order;
}
m--;
// Apply this part of the filter
// Equivalent to:
// output[dd] += ab.a * xyz.a;
// output[dd] += ab.b * xyz.b;
ab_t ab = state->abs[k];
ab_t xyz = xy[m];
output_val = __smlal(output_val, bitsk(ab.a), bitsk(xyz.a));
output_val = __smlal(output_val, bitsk(ab.b), bitsk(xyz.b));
}
// Include the initial new input
xy[state->n].b = input[dd];
// Save the current output for later steps
output[dd] = kbits(convert_s32_30_s16_15(output_val));
xy[state->n].a = output[dd];
}
// Rotate the ring buffer by moving the starting pointer, if the starting
// pointer would go beyond the end of the buffer it is returned to the start.
if (++state->n == state->order)
{
state->n = 0;
}
}
