| #include "ref.h" |
| |
| float32_t ref_pid_f32( |
| arm_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( |
| arm_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( |
| arm_pid_instance_q15 * S, |
| q15_t in) |
| { |
| q63_t acc; |
| q15_t out; |
| q15_t A1, A2; |
| |
| #if defined (ARM_MATH_DSP) |
| |
| #ifndef ARM_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->A2; |
| |
| #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); |
| } |