[DO NOT MERGE] vibrator: Fix the defect in calibration process

1. Add output protection for the rest of cases
2. Add boundary protection for the maximum level
3. Add a linear approach for 2 coeffs cases

Bug: 147206963
Bug: 147784756
Test: manual check calibration output values
Change-Id: I7d415b39b58ed94a152c3a4729fb172711cea7dc
Signed-off-by: chasewu <chasewu@google.com>

7 files changed, 110 insertions, 34 deletions

diff --git a/vibrator/Android.bp b/vibrator/Android.bp
index aadd9152..cd89f0c1 100644
--- a/vibrator/Android.bp
+++ b/vibrator/Android.bp
@@ -14,11 +14,11 @@
 // limitations under the License.
 
 cc_defaults {
-    name: "PixelVibratorDefaults",
+    name: "PixelVibratorDefaultsSunfish",
     defaults: ["hidl_defaults"],
     relative_install_path: "hw",
     static_libs: [
-        "PixelVibratorCommon",
+        "PixelVibratorCommonSunfish",
     ],
     shared_libs: [
         "libbase",
diff --git a/vibrator/common/Android.bp b/vibrator/common/Android.bp
index d40e7a91..a62d43b3 100644
--- a/vibrator/common/Android.bp
+++ b/vibrator/common/Android.bp
@@ -14,7 +14,7 @@
 // limitations under the License.
 
 cc_library {
-    name: "PixelVibratorCommon",
+    name: "PixelVibratorCommonSunfish",
     srcs: [
         "HardwareBase.cpp",
     ],
diff --git a/vibrator/common/bench/Android.bp b/vibrator/common/bench/Android.bp
index 641b73ab..aa582ef8 100644
--- a/vibrator/common/bench/Android.bp
+++ b/vibrator/common/bench/Android.bp
@@ -14,7 +14,7 @@
 // limitations under the License.
 
 cc_benchmark {
-    name: "VibratorHalIntegrationBenchmark",
+    name: "VibratorHalIntegrationBenchmarkSunfish",
     defaults: ["hidl_defaults"],
     srcs: [
         "benchmark.cpp",
diff --git a/vibrator/drv2624/Android.bp b/vibrator/drv2624/Android.bp
index 74badc6a..039010c4 100644
--- a/vibrator/drv2624/Android.bp
+++ b/vibrator/drv2624/Android.bp
@@ -15,7 +15,7 @@
 
 cc_defaults {
     name: "android.hardware.vibrator@1.3-defaults.sunfish",
-    defaults: ["PixelVibratorDefaults"],
+    defaults: ["PixelVibratorDefaultsSunfish"],
     shared_libs: [
         "android.hardware.vibrator@1.0",
         "android.hardware.vibrator@1.1",
@@ -29,7 +29,7 @@ cc_defaults {
 }
 
 cc_defaults {
-    name: "VibratorHalDrv2624TestDefaults",
+    name: "VibratorHalDrv2624TestDefaultsSunfish",
     defaults: ["android.hardware.vibrator@1.3-defaults.sunfish"],
     static_libs: ["android.hardware.vibrator@1.3-impl.sunfish"],
     test_suites: ["device-tests"],
diff --git a/vibrator/drv2624/Vibrator.cpp b/vibrator/drv2624/Vibrator.cpp
index ae7db16f..a5b97a76 100644
--- a/vibrator/drv2624/Vibrator.cpp
+++ b/vibrator/drv2624/Vibrator.cpp
@@ -57,6 +57,8 @@ static constexpr char WAVEFORM_HEAVY_CLICK_EFFECT_SEQ[] = "4 0";
 static constexpr std::array<float, 5> EFFECT_TARGET_G = {0.15, 0.15, 0.27, 0.43, 0.57};
 static constexpr std::array<float, 3> STEADY_TARGET_G = {1.2, 1.145, 0.905};
 
+#define FLOAT_EPS 1e-6
+
 static std::uint32_t freqPeriodFormula(std::uint32_t in) {
     return 1000000000 / (24615 * in);
 }
@@ -71,38 +73,97 @@ static std::uint32_t convertLevelsToOdClamp(float voltageLevel, uint32_t lraPeri
     return round(odClamp);
 }
 
-static float convertTargetGToVlevels(std::array<float, 4> inputCoeffs, float targetG) {
-    // implement cubic equation to get voltage levels
+static float targetGToVlevelsUnderLinearEquation(std::array<float, 4> inputCoeffs, float targetG) {
+    // Implement linear equation to get voltage levels, f(x) = ax + b
+    // 0 to 3.2 is our valid output
+    float outPutVal = 0.0f;
+    outPutVal = (targetG - inputCoeffs[1]) / inputCoeffs[0];
+    if ((outPutVal > FLOAT_EPS) && (outPutVal <= 3.2)) {
+        return outPutVal;
+    } else {
+        return 0.0f;
+    }
+}
+
+static float targetGToVlevelsUnderCubicEquation(std::array<float, 4> inputCoeffs, float targetG) {
+    // Implement cubic equation to get voltage levels, f(x) = ax^3 + bx^2 + cx + d
+    // 0 to 3.2 is our valid output
     float AA = 0.0f, BB = 0.0f, CC = 0.0f, Delta = 0.0f;
-    float Y1 = 0.0f, Y2 = 0.0f;
-    float T = 0.0f, sita = 0.0f;
+    float Y1 = 0.0f, Y2 = 0.0f, K = 0.0f, T = 0.0f, sita = 0.0f;
+    float outPutVal = 0.0f;
+    float oneHalf = 1.0 / 2.0, oneThird = 1.0 / 3.0;
+    float cosSita = 0.0f, sinSitaSqrt3 = 0.0f, sqrtA = 0.0f;
 
     AA = inputCoeffs[1] * inputCoeffs[1] - 3.0 * inputCoeffs[0] * inputCoeffs[2];
     BB = inputCoeffs[1] * inputCoeffs[2] - 9.0 * inputCoeffs[0] * (inputCoeffs[3] - targetG);
     CC = inputCoeffs[2] * inputCoeffs[2] - 3.0 * inputCoeffs[1] * (inputCoeffs[3] - targetG);
 
     Delta = BB * BB - 4.0 * AA * CC;
-    if (Delta < 0) {
+
+    // There are four discriminants in Shengjin formula.
+    // https://zh.wikipedia.org/wiki/%E4%B8%89%E6%AC%A1%E6%96%B9%E7%A8%8B#%E7%9B%9B%E9%87%91%E5%85%AC%E5%BC%8F%E6%B3%95
+    if ((fabs(AA) <= FLOAT_EPS) && (fabs(BB) <= FLOAT_EPS)) {
+        // Case 1: A = B = 0
+        outPutVal = -inputCoeffs[1] / (3 * inputCoeffs[0]);
+        if ((outPutVal > FLOAT_EPS) && (outPutVal <= 3.2)) {
+            return outPutVal;
+        }
+        return 0.0f;
+    } else if (Delta > FLOAT_EPS) {
+        // Case 2: Delta > 0
+        Y1 = AA * inputCoeffs[1] + 3.0 * inputCoeffs[0] * (-BB + pow(Delta, oneHalf)) / 2.0;
+        Y2 = AA * inputCoeffs[1] + 3.0 * inputCoeffs[0] * (-BB - pow(Delta, oneHalf)) / 2.0;
+
+        if ((Y1 < -FLOAT_EPS) && (Y2 > FLOAT_EPS)) {
+            return (-inputCoeffs[1] + pow(-Y1, oneThird) - pow(Y2, oneThird)) /
+                   (3.0 * inputCoeffs[0]);
+        } else if ((Y1 > FLOAT_EPS) && (Y2 < -FLOAT_EPS)) {
+            return (-inputCoeffs[1] - pow(Y1, oneThird) + pow(-Y2, oneThird)) /
+                   (3.0 * inputCoeffs[0]);
+        } else if ((Y1 < -FLOAT_EPS) && (Y2 < -FLOAT_EPS)) {
+            return (-inputCoeffs[1] + pow(-Y1, oneThird) + pow(-Y2, oneThird)) /
+                   (3.0 * inputCoeffs[0]);
+        } else {
+            return (-inputCoeffs[1] - pow(Y1, oneThird) - pow(Y2, oneThird)) /
+                   (3.0 * inputCoeffs[0]);
+        }
+        return 0.0f;
+    } else if (Delta < -FLOAT_EPS) {
+        // Case 3: Delta < 0
         T = (2 * AA * inputCoeffs[1] - 3 * inputCoeffs[0] * BB) / (2 * AA * sqrt(AA));
         sita = acos(T);
-        return (-inputCoeffs[1] + sqrt(AA) * (cos(sita / 3) - sqrt(3.0) * sin(sita / 3))) /
-               (3 * inputCoeffs[0]);
-    }
+        cosSita = cos(sita / 3);
+        sinSitaSqrt3 = sqrt(3.0) * sin(sita / 3);
+        sqrtA = sqrt(AA);
 
-    Y1 = AA * inputCoeffs[1] + 3.0 * inputCoeffs[0] * (-BB + pow(Delta, 1.0 / 2.0)) / 2.0;
-    Y2 = AA * inputCoeffs[1] + 3.0 * inputCoeffs[0] * (-BB - pow(Delta, 1.0 / 2.0)) / 2.0;
-
-    if ((Y1 < 0) && (Y2 > 0)) {
-        return (-inputCoeffs[1] + pow(-Y1, 1.0 / 3.0) - pow(Y2, 1.0 / 3.0)) /
-               (3.0 * inputCoeffs[0]);
-    } else if ((Y1 > 0) && (Y2 < 0)) {
-        return (-inputCoeffs[1] - pow(Y1, 1.0 / 3.0) + pow(-Y2, 1.0 / 3.0)) /
-               (3.0 * inputCoeffs[0]);
-    } else if ((Y1 < 0) && (Y2 < 0)) {
-        return (-inputCoeffs[1] + pow(-Y1, 1.0 / 3.0) + pow(-Y2, 1.0 / 3.0)) /
-               (3.0 * inputCoeffs[0]);
+        outPutVal = (-inputCoeffs[1] - 2 * sqrtA * cosSita) / (3 * inputCoeffs[0]);
+        if ((outPutVal > FLOAT_EPS) && (outPutVal <= 3.2)) {
+            return outPutVal;
+        }
+        outPutVal = (-inputCoeffs[1] + sqrtA * (cosSita + sinSitaSqrt3)) / (3 * inputCoeffs[0]);
+        if ((outPutVal > FLOAT_EPS) && (outPutVal <= 3.2)) {
+            return outPutVal;
+        }
+        outPutVal = (-inputCoeffs[1] + sqrtA * (cosSita - sinSitaSqrt3)) / (3 * inputCoeffs[0]);
+        if ((outPutVal > FLOAT_EPS) && (outPutVal <= 3.2)) {
+            return outPutVal;
+        }
+        return 0.0f;
+    } else if (Delta <= FLOAT_EPS) {
+        // Case 4: Delta = 0
+        K = BB / AA;
+        outPutVal = (-inputCoeffs[1] / inputCoeffs[0] + K);
+        if ((outPutVal > FLOAT_EPS) && (outPutVal <= 3.2)) {
+            return outPutVal;
+        }
+        outPutVal = (-K / 2);
+        if ((outPutVal > FLOAT_EPS) && (outPutVal <= 3.2)) {
+            return outPutVal;
+        }
+        return 0.0f;
     } else {
-        return (-inputCoeffs[1] - pow(Y1, 1.0 / 3.0) - pow(Y2, 1.0 / 3.0)) / (3.0 * inputCoeffs[0]);
+        // Exception handling
+        return 0.0f;
     }
 }
 
@@ -117,7 +178,7 @@ Vibrator::Vibrator(std::unique_ptr<HwApi> hwapi, std::unique_ptr<HwCal> hwcal)
     uint32_t lraPeriod = 0;
     bool dynamicConfig = false;
     bool hasEffectCoeffs = false;
-    std::array<float, 4> effectCoeffs = {};
+    std::array<float, 4> effectCoeffs = {0};
 
     if (!mHwApi->setState(true)) {
         ALOGE("Failed to set state (%d): %s", errno, strerror(errno));
@@ -146,12 +207,27 @@ Vibrator::Vibrator(std::unique_ptr<HwApi> hwapi, std::unique_ptr<HwCal> hwcal)
         hasEffectCoeffs = mHwCal->getEffectCoeffs(&effectCoeffs);
         for (i = 0; i < 5; i++) {
             if (hasEffectCoeffs) {
-                tempVolLevel = convertTargetGToVlevels(effectCoeffs, EFFECT_TARGET_G[i]);
-                mEffectTargetOdClamp[i] = convertLevelsToOdClamp(tempVolLevel, lraPeriod);
+                // Use linear approach to get the target voltage levels
+                if ((effectCoeffs[2] == 0) && (effectCoeffs[3] == 0)) {
+                    tempVolLevel =
+                        targetGToVlevelsUnderLinearEquation(effectCoeffs, EFFECT_TARGET_G[i]);
+                    mEffectTargetOdClamp[i] = convertLevelsToOdClamp(tempVolLevel, lraPeriod);
+                } else {
+                    // Use cubic approach to get the target voltage levels
+                    tempVolLevel =
+                        targetGToVlevelsUnderCubicEquation(effectCoeffs, EFFECT_TARGET_G[i]);
+                    mEffectTargetOdClamp[i] = convertLevelsToOdClamp(tempVolLevel, lraPeriod);
+                }
             } else {
                 mEffectTargetOdClamp[i] = shortVoltageMax;
             }
         }
+        // Add a boundary protection for level 5 only, since
+        // some devices might not be able to reach the maximum target G
+        if ((mEffectTargetOdClamp[4] <= 0) || (mEffectTargetOdClamp[4] > 161)) {
+            mEffectTargetOdClamp[4] = shortVoltageMax;
+        }
+
         mHwCal->getEffectShape(&shape);
         mEffectConfig.reset(new VibrationConfig({
             .shape = (shape == UINT32_MAX) ? WaveShape::SINE : static_cast<WaveShape>(shape),
diff --git a/vibrator/drv2624/bench/Android.bp b/vibrator/drv2624/bench/Android.bp
index 6227dc5a..f79d0393 100644
--- a/vibrator/drv2624/bench/Android.bp
+++ b/vibrator/drv2624/bench/Android.bp
@@ -14,8 +14,8 @@
 // limitations under the License.
 
 cc_benchmark {
-    name: "VibratorHalDrv2624Benchmark",
-    defaults: ["VibratorHalDrv2624TestDefaults"],
+    name: "VibratorHalDrv2624BenchmarkSunfish",
+    defaults: ["VibratorHalDrv2624TestDefaultsSunfish"],
     srcs: [
         "benchmark.cpp",
     ],
diff --git a/vibrator/drv2624/tests/Android.bp b/vibrator/drv2624/tests/Android.bp
index 4c857402..d000c10c 100644
--- a/vibrator/drv2624/tests/Android.bp
+++ b/vibrator/drv2624/tests/Android.bp
@@ -14,8 +14,8 @@
 // limitations under the License.
 
 cc_test {
-    name: "VibratorHalDrv2624TestSuite",
-    defaults: ["VibratorHalDrv2624TestDefaults"],
+    name: "VibratorHalDrv2624TestSuiteSunfish",
+    defaults: ["VibratorHalDrv2624TestDefaultsSunfish"],
     srcs: [
         "test-hwapi.cpp",
         "test-hwcal.cpp",