From 6ab94e0b318884bbcb95e2ea3835f951502e1d99 Mon Sep 17 00:00:00 2001
From: jaseg <git@jaseg.net>
Date: Wed, 14 Oct 2020 12:47:28 +0200
Subject: Move firmware into subdirectory

---
 .../RefLibs/src/ControllerFunctions/pid.c          | 97 ++++++++++++++++++++++
 .../RefLibs/src/ControllerFunctions/sin_cos.c      | 21 +++++
 2 files changed, 118 insertions(+)
 create mode 100644 fw/midi-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/ControllerFunctions/pid.c
 create mode 100644 fw/midi-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/ControllerFunctions/sin_cos.c

(limited to 'fw/midi-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/ControllerFunctions')

diff --git a/fw/midi-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/ControllerFunctions/pid.c b/fw/midi-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/ControllerFunctions/pid.c
new file mode 100644
index 0000000..51aa633
--- /dev/null
+++ b/fw/midi-dials/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/fw/midi-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/ControllerFunctions/sin_cos.c b/fw/midi-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/ControllerFunctions/sin_cos.c
new file mode 100644
index 0000000..22c91a0
--- /dev/null
+++ b/fw/midi-dials/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);
+}
-- 
cgit