add lut exp to driver

1 files changed, 25 insertions, 18 deletions

diff --git a/common/accel-experimenttwo.hpp b/common/accel-experimenttwo.hpp
index 73428f7..a961f78 100644
--- a/common/accel-experimenttwo.hpp
+++ b/common/accel-experimenttwo.hpp
@@ -4,8 +4,17 @@
 
 #include "accel-base.hpp"
 
+#define RA_LOOKUP
+
 namespace rawaccel {
 
+	constexpr size_t LUT_SIZE = 601;
+
+	struct si_pair { 
+		double slope = 0;
+		double intercept = 0; 
+	};
+
 	/// <summary> Struct to hold sigmoid (s-shaped) gain implementation. </summary>
 	struct experimenttwo_impl {
 		double rate;
@@ -27,51 +36,49 @@ namespace rawaccel {
 
 		inline double legacy_offset(double speed) const { return operator()(speed); }
 
-		inline double apply(double* lookup, double speed)
+		inline double apply(si_pair* lookup, double speed) const
 		{
-			int index = map(speed);
-			double slope = lookup[index];
-			double intercept = lookup[index + 1];
-			return slope + intercept / speed;
+			si_pair pair = lookup[map(speed)];
+			return pair.slope + pair.intercept / speed;
 		}
 
-		inline int map(double speed)
+		inline int map(double speed) const
 		{
-			int index = speed > 0 ? (int)floor(200*log10(speed)+402) : 0;
+			int index = speed > 0 ? (int)(100 * log10(speed) + 201) : 0;
 
 			if (index < 0) return 0;
-			if (index > 1200) return 1200;
+			if (index >= LUT_SIZE) return LUT_SIZE - 1;
 
 			return index;
 		}
 
-		inline double fill(double* lookup)
+		inline void fill(si_pair* lookup) const
 		{
 			double lookup_speed = 0;
 			double gain_integral_speed = 0;
 			double gain = 0;
 			double intercept = 0;
 			double output = 0;
-			int index = 0;
+			double x = -2;
 
-			lookup[index] = gain;
-			lookup[index + 1] = intercept;
+			lookup[0] = {};
 
-			for (double x = -2.0; x <= 4.0; x += 0.01)
+			for (size_t i = 1; i < LUT_SIZE; i++)
 			{
-				index+=2;
-				lookup_speed = pow(10,x);
+				x += 0.01;
+
+				lookup_speed = pow(10, x);
 
 				while (gain_integral_speed < lookup_speed)
 				{
 					gain_integral_speed += 0.001;
 					gain = operator()(gain_integral_speed);
-					output += gain*0.001;
+					output += gain * 0.001;
 				}
 
 				intercept = gain * lookup_speed - output;
-				lookup[index] = gain;
-				lookup[index + 1] = intercept;
+
+				lookup[i] = { gain, intercept };
 			}
 
 		}