92 lines
1.7 KiB
C
92 lines
1.7 KiB
C
|
#include "ref.h"
|
||
|
|
|
||
|
|
float32_t ref_pid_f32(riscv_pid_instance_f32 *S, float32_t in)
|
||
|
|
{
|
||
|
|
float32_t out;
|
||
|
|
|
||
|
|
/* y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2] */
|
||
|
|
out = S->state[2] + S->A0 * in + S->A1 * S->state[0] + S->A2 * S->state[1];
|
||
|
|
|
||
|
|
/* Update state */
|
||
|
|
S->state[1] = S->state[0];
|
||
|
|
S->state[0] = in;
|
||
|
|
S->state[2] = out;
|
||
|
|
|
||
|
|
/* return to application */
|
||
|
|
return (out);
|
||
|
|
}
|
||
|
|
|
||
|
|
q31_t ref_pid_q31(riscv_pid_instance_q31 *S, q31_t in)
|
||
|
|
{
|
||
|
|
q63_t acc;
|
||
|
|
q31_t out;
|
||
|
|
|
||
|
|
/* acc = A0 * x[n] */
|
||
|
|
acc = (q63_t)S->A0 * in;
|
||
|
|
|
||
|
|
/* acc += A1 * x[n-1] */
|
||
|
|
acc += (q63_t)S->A1 * S->state[0];
|
||
|
|
|
||
|
|
/* acc += A2 * x[n-2] */
|
||
|
|
acc += (q63_t)S->A2 * S->state[1];
|
||
|
|
|
||
|
|
/* convert output to 1.31 format to add y[n-1] */
|
||
|
|
out = (q31_t)(acc >> 31U);
|
||
|
|
|
||
|
|
/* out += y[n-1] */
|
||
|
|
out += S->state[2];
|
||
|
|
|
||
|
|
/* Update state */
|
||
|
|
S->state[1] = S->state[0];
|
||
|
|
S->state[0] = in;
|
||
|
|
S->state[2] = out;
|
||
|
|
|
||
|
|
/* return to application */
|
||
|
|
return (out);
|
||
|
|
}
|
||
|
|
|
||
|
|
q15_t ref_pid_q15(riscv_pid_instance_q15 *S, q15_t in)
|
||
|
|
{
|
||
|
|
q63_t acc;
|
||
|
|
q15_t out;
|
||
|
|
q15_t A1, A2;
|
||
|
|
|
||
|
|
#if defined(riscv_MATH_DSP)
|
||
|
|
|
||
|
|
#ifndef riscv_MATH_BIG_ENDIAN
|
||
|
|
A2 = S->A1 >> 16;
|
||
|
|
A1 = (q15_t)S->A1;
|
||
|
|
#else
|
||
|
|
A1 = S->A1 >> 16;
|
||
|
|
A2 = (q15_t)S->A1;
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#else
|
||
|
|
|
||
|
|
A1 = S->A1;
|
||
|
|
A2 = S->Kd;
|
||
|
|
|
||
|
|
#endif
|
||
|
|
|
||
|
|
/* acc = A0 * x[n] */
|
||
|
|
acc = ((q31_t)S->A0) * in;
|
||
|
|
|
||
|
|
/* acc += A1 * x[n-1] + A2 * x[n-2] */
|
||
|
|
acc += (q31_t)A1 * S->state[0];
|
||
|
|
acc += (q31_t)A2 * S->state[1];
|
||
|
|
|
||
|
|
/* acc += y[n-1] */
|
||
|
|
acc += (q31_t)S->state[2] << 15;
|
||
|
|
|
||
|
|
/* saturate the output */
|
||
|
|
out = ref_sat_q15(acc >> 15);
|
||
|
|
|
||
|
|
/* Update state */
|
||
|
|
S->state[1] = S->state[0];
|
||
|
|
S->state[0] = in;
|
||
|
|
S->state[2] = out;
|
||
|
|
|
||
|
|
/* return to application */
|
||
|
|
return (out);
|
||
|
|
}
|