From 9f3c8dddd39b503a5e1b085e7a62397ddf07c68a Mon Sep 17 00:00:00 2001
From: James Cotton <peabody124@gmail.com>
Date: Fri, 3 Aug 2012 12:15:57 -0500
Subject: [PATCH] Add a version of the sin lookup table that is in ram instead
 of flash

---
 flight/Libraries/math/sin_lookup.c           | 50 ++++++++++++++++++++
 flight/Libraries/math/sin_lookup.h           |  1 +
 flight/Modules/Stabilization/relay_tuning.c  |  4 +-
 flight/Modules/Stabilization/stabilization.c | 10 ++--
 4 files changed, 60 insertions(+), 5 deletions(-)

diff --git a/flight/Libraries/math/sin_lookup.c b/flight/Libraries/math/sin_lookup.c
index 575f171d3..2a5508fce 100644
--- a/flight/Libraries/math/sin_lookup.c
+++ b/flight/Libraries/math/sin_lookup.c
@@ -28,9 +28,14 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
+#include "openpilot.h"
 #include "math.h"
+#include "stdbool.h"
 #include "stdint.h"
 
+#ifdef FLASH_TABLE
+ /** Version of the code which precomputes the lookup table in flash **/
+
  // This is a precomputed sin lookup table over 90 degrees that
  const float sin_table[] = {
  	0.000000f,0.017452f,0.034899f,0.052336f,0.069756f,0.087156f,0.104528f,0.121869f,0.139173f,0.156434f,
@@ -44,6 +49,11 @@
  	0.984808f,0.987688f,0.990268f,0.992546f,0.994522f,0.996195f,0.997564f,0.998630f,0.999391f,0.999848f
  };
 
+int sin_lookup_initalize()
+{
+	return 0;
+}
+
 /**
  * Use the lookup table to return sine(angle) where angle is in radians
  * to save flash this cheats and uses trig functions to only save
@@ -66,6 +76,46 @@ float sin_lookup_deg(float angle)
 	return 0;
 }
 
+#else
+/** Version of the code which allocates the lookup table in heap **/
+
+const int SIN_RESOLUTION = 180;
+
+static float *sin_table;
+int sin_lookup_initalize()
+{
+	// Previously initialized
+	if (sin_table)
+		return 0;
+
+	sin_table = (float *) pvPortMalloc(sizeof(float) * SIN_RESOLUTION);
+	if (sin_table == NULL)
+		return -1;
+
+	for(uint32_t i = 0; i < 180; i++)
+		sin_table[i] = sinf((float)i * 2 * M_PI / 360.0f);
+
+	return 0;
+}
+
+
+ 
+/**
+ * Uses the lookup table to calculate sine (angle is in degrees)
+ * @param[in] angle in degrees
+ * @returns sin(angle)
+ */
+float sin_lookup_deg(float angle) {
+	int i_ang = ((int32_t)angle) % 360;
+	if (i_ang > 180)
+		return - sin_table[i_ang-180];
+	else
+	return sin_table[i_ang];
+}
+
+#endif
+
+
 /**
  * Get cos(angle) using the sine lookup table
  * @param[in] angle Angle in degrees
diff --git a/flight/Libraries/math/sin_lookup.h b/flight/Libraries/math/sin_lookup.h
index 0e0edb274..adc29f66a 100644
--- a/flight/Libraries/math/sin_lookup.h
+++ b/flight/Libraries/math/sin_lookup.h
@@ -31,6 +31,7 @@
 #ifndef SIN_LOOKUP_H
 #define SIN_LOOKUP_H
 
+int sin_lookup_initalize();
 float sin_lookup_deg(float angle);
 float cos_lookup_deg(float angle);
 float sin_lookup_rad(float angle);
diff --git a/flight/Modules/Stabilization/relay_tuning.c b/flight/Modules/Stabilization/relay_tuning.c
index 5dc538b4a..0f0bb9d55 100644
--- a/flight/Modules/Stabilization/relay_tuning.c
+++ b/flight/Modules/Stabilization/relay_tuning.c
@@ -101,11 +101,11 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
 	// Project the error onto a sine and cosine of the same frequency
 	// to accumulate the average amplitude
 	int32_t dT = thisTime - lastHighTime;
-	int32_t phase = (360 * dT) / relay.Period[axis];
+	float phase = ((float)360 * (float)dT) / relay.Period[axis];
 	if(phase >= 360)
 		phase = 0;
 	accum_sin += sin_lookup_deg(phase) * error;
-	accum_cos += cos_lookup_deg(phase) * error;
+	accum_cos += sin_lookup_deg(phase + 90) * error;
 	accumulated ++;
 
 	// Make sure we've had enough time since last transition then check for a change in the output
diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c
index 188f556c8..c4cc801e8 100644
--- a/flight/Modules/Stabilization/stabilization.c
+++ b/flight/Modules/Stabilization/stabilization.c
@@ -47,6 +47,7 @@
 // Math libraries
 #include "CoordinateConversions.h"
 #include "pid.h"
+#include "sin_lookup.h"
 
 // Includes for various stabilization algorithms
 #include "relay_tuning.h"
@@ -117,12 +118,15 @@ int32_t StabilizationInitialize()
 	// Initialize variables
 	StabilizationSettingsInitialize();
 	ActuatorDesiredInitialize();
-	RelayTuningSettingsInitialize();
-	RelayTuningInitialize();
 #if defined(DIAGNOSTICS)
 	RateDesiredInitialize();
 #endif
-	
+
+	// Code required for relay tuning
+	sin_lookup_initalize();
+	RelayTuningSettingsInitialize();
+	RelayTuningInitialize();
+
 	return 0;
 }