From 747b3d2fa862013b0b8fa6cbc462e11c20c34a33 Mon Sep 17 00:00:00 2001
From: Matthew Kennedy <matthewkennedy@outlook.com>
Date: Sun, 22 Nov 2020 05:26:17 -0800
Subject: [PATCH] fix #1968 (#1976)

* tanf

* name

* comment, add test
---
 firmware/util/efilib.cpp                   | 26 ++++++++++++++++++++++
 firmware/util/efilib.h                     |  4 ++++
 firmware/util/math/biquad.cpp              |  4 ++--
 unit_tests/test_basic_math/test_efilib.cpp |  9 ++++++++
 4 files changed, 41 insertions(+), 2 deletions(-)

diff --git a/firmware/util/efilib.cpp b/firmware/util/efilib.cpp
index 72e2fda824..dd5f27ee03 100644
--- a/firmware/util/efilib.cpp
+++ b/firmware/util/efilib.cpp
@@ -371,3 +371,29 @@ float expf_taylor(float x)
 {
 	return expf_taylor_impl(x, 4);
 }
+
+float tanf_taylor(float x) {
+	// This exists because the "normal" implementation, tanf, pulls in like 6kb of
+	// code and loookup tables
+
+	// This is only specified from [0, pi/2 - 0.01)
+	// Inside that range it has an error of less than 0.1%, and it gets worse as theta -> pi/2
+
+	// Precompute some exponents of x
+	float x2 = x * x;
+	float x3 = x2 * x;
+	float x4 = x3 * x;
+	float x5 = x4 * x;
+	float x6 = x5 * x;
+	// x7 not used
+	float x8 = x6 * x2;
+
+	// 3-term Taylor Series for sin(theta)
+	float sin_val = x - (x3 / 6) + (x5 / 120);
+
+	// 5-term Taylor Series for cos(theta)
+	float cos_val = 1 - (x2 / 2) + (x4 / 24) - (x6 / 720) + (x8 / 40320);
+
+	// tan = sin / cos
+	return sin_val / cos_val;
+}
diff --git a/firmware/util/efilib.h b/firmware/util/efilib.h
index a546f64898..e532f12108 100644
--- a/firmware/util/efilib.h
+++ b/firmware/util/efilib.h
@@ -84,6 +84,10 @@ float limitRateOfChange(float newValue, float oldValue, float incrLimitPerSec, f
 // bogus results outside the range -2 < x < 0.
 float expf_taylor(float x);
 
+// @brief Compute tan(theta) using a ratio of the Taylor series for sin and cos
+// Valid for the range [0, pi/2 - 0.01]
+float tanf_taylor(float theta);
+
 #ifdef __cplusplus
 }
 
diff --git a/firmware/util/math/biquad.cpp b/firmware/util/math/biquad.cpp
index 61091757ad..3dd18eedd7 100644
--- a/firmware/util/math/biquad.cpp
+++ b/firmware/util/math/biquad.cpp
@@ -8,7 +8,7 @@
 #include "biquad.h"
 #include "error_handling.h"
 
-#include <math.h>
+#include "efilib.h"
 
 Biquad::Biquad() {
 // Default to passthru
@@ -23,7 +23,7 @@ void Biquad::reset() {
 }
 
 static float getK(float samplingFrequency, float cutoff) {
-	return tanf(3.14159f * cutoff / samplingFrequency);
+	return tanf_taylor(3.14159f * cutoff / samplingFrequency);
 }
 
 static float getNorm(float K, float Q) {
diff --git a/unit_tests/test_basic_math/test_efilib.cpp b/unit_tests/test_basic_math/test_efilib.cpp
index e6070a4b58..072e255718 100644
--- a/unit_tests/test_basic_math/test_efilib.cpp
+++ b/unit_tests/test_basic_math/test_efilib.cpp
@@ -36,3 +36,12 @@ TEST(EfiLibTest, clampf) {
 	EXPECT_EQ(clampF(-10, 50, 10), 10);
 
 }
+
+TEST(EfiLibTest, tanf_taylor) {
+	// Function is only specified from [0, pi/2) ish, so test that range
+	for (float i = 0; i < 1.5; i += 0.1f)
+	{
+		// Compare to libc implementation
+		EXPECT_NEAR(tanf_taylor(i), tanf(i), 0.05f) << "I = " << i;
+	}
+}