Expand the sin/cos tables to 4096
The pistol grip controller has encoders with more counts.
Change-Id: I8bad6c7c2c5b1ba33bd576a3fd71cbd610568917
diff --git a/motors/math.h b/motors/math.h
index 851a90a..fce3a56 100644
--- a/motors/math.h
+++ b/motors/math.h
@@ -23,7 +23,7 @@
namespace math_internal {
-constexpr uint32_t SinCosTableSize() { return 1024; }
+constexpr uint32_t SinCosTableSize() { return 4096; }
constexpr float FloatMaxMagnitude() { return 1.0f; }
@@ -70,9 +70,10 @@
}
inline float FastTableLookupFloat(float theta, const float *table) {
- const int index = (SinCosTableSize() / 2) +
- static_cast<int32_t>(theta * ((SinCosTableSize() / 2) /
- FloatMaxMagnitude()));
+ static constexpr float kScalar =
+ (SinCosTableSize() / 2) / FloatMaxMagnitude();
+ const int index =
+ (SinCosTableSize() / 2) + static_cast<int32_t>(theta * kScalar);
return table[index];
}