diff options
| author | JanHenrik <janhenrik@janhenrik.org> | 2020-01-19 00:56:37 +0100 |
|---|---|---|
| committer | JanHenrik <janhenrik@janhenrik.org> | 2020-01-19 00:56:37 +0100 |
| commit | a81fc41c3eb99e8fc52aa734ee63e13c937aab81 (patch) | |
| tree | 19974c4676ed64d6e4260b79fc3f1eaeda58a289 /Blink/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/ControllerFunctions | |
| parent | a96ec5d9995e7e4b7315a192e543b88f0aff4930 (diff) | |
| download | minikbd-a81fc41c3eb99e8fc52aa734ee63e13c937aab81.tar.gz minikbd-a81fc41c3eb99e8fc52aa734ee63e13c937aab81.tar.bz2 minikbd-a81fc41c3eb99e8fc52aa734ee63e13c937aab81.zip | |
added blink example
Diffstat (limited to 'Blink/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/ControllerFunctions')
| -rw-r--r-- | Blink/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/ControllerFunctions/pid.c | 97 | ||||
| -rw-r--r-- | Blink/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/ControllerFunctions/sin_cos.c | 21 |
2 files changed, 118 insertions, 0 deletions
diff --git a/Blink/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/ControllerFunctions/pid.c b/Blink/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/ControllerFunctions/pid.c new file mode 100644 index 0000000..51aa633 --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/ControllerFunctions/pid.c @@ -0,0 +1,97 @@ +#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);
+}
diff --git a/Blink/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/ControllerFunctions/sin_cos.c b/Blink/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/ControllerFunctions/sin_cos.c new file mode 100644 index 0000000..22c91a0 --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/ControllerFunctions/sin_cos.c @@ -0,0 +1,21 @@ +#include "ref.h"
+
+void ref_sin_cos_f32(
+ float32_t theta,
+ float32_t * pSinVal,
+ float32_t * pCosVal)
+{
+ //theta is given in degrees
+ *pSinVal = sinf(theta * 6.28318530717959f / 360.0f);
+ *pCosVal = cosf(theta * 6.28318530717959f / 360.0f);
+}
+
+void ref_sin_cos_q31(
+ q31_t theta,
+ q31_t * pSinVal,
+ q31_t * pCosVal)
+{
+ //theta is given in the range [-1,1) to represent [-pi,pi)
+ *pSinVal = (q31_t)(sinf((float32_t)theta * 3.14159265358979f / 2147483648.0f) * 2147483648.0f);
+ *pCosVal = (q31_t)(cosf((float32_t)theta * 3.14159265358979f / 2147483648.0f) * 2147483648.0f);
+}
|