1 files changed, 48 insertions, 0 deletions
diff --git a/core/math.c b/core/math.c
new file mode 100644
index 0000000..f962534
--- /dev/null
+++ b/core/math.c
@@ -0,0 +1,48 @@
+#include "math.h"
+
+
+float to_float(uint8_t x) 
+{
+	return (1.0f/127.0f)*(((float)x)-127.0f);
+}
+
+
+//float ph_ch( uint8_t i1, uint8_t q1, uint8_t i2, uint8_t q2)
+float ph_ch( uint8_t i1, uint8_t q1 )
+{
+	static float complex last=0.0+0.0i;
+	float out;
+	float complex xy,c1;
+	//float c2;
+
+	c1 = to_float(i1) + I*to_float(q1);
+	//c1 = CMPLXF( to_float(i1), to_float(q1) );
+	//c2 = to_float(i2) + I*to_float(q2);
+	//c2 = CMPLXF( to_float(i2), to_float(q2) );
+	xy = conjf(last)*c1;
+	out = cargf( xy );
+	last = c1;
+
+	return out;
+}
+
+void rotate_90(uint8_t *buf, uint32_t len)
+/* 90 rotation is 1+0j, 0+1j, -1+0j, 0-1j
+   or [0, 1, -3, 2, -4, -5, 7, -6] */
+{
+	uint32_t i;
+	uint8_t tmp;
+	for (i=0; i<len; i+=8) {
+		/* uint8_t negation = 255 - x */
+		tmp = 255 - buf[i+3];
+		buf[i+3] = buf[i+2];
+		buf[i+2] = tmp;
+
+		buf[i+4] = 255 - buf[i+4];
+		buf[i+5] = 255 - buf[i+5];
+
+		tmp = 255 - buf[i+6];
+		buf[i+6] = buf[i+7];
+		buf[i+7] = tmp;
+	}
+}
+\ No newline at end of file