vibrator: Use interpolation method for non-motion voltage

Bug: 156428459
Test: manual check the ol_clamp
Signed-off-by: chasewu <chasewu@google.com>
Change-Id: I3ef918098391a08d4ed36e057f6ce093a702d924

1 files changed, 30 insertions, 9 deletions

diff --git a/vibrator/drv2624/Vibrator.cpp b/vibrator/drv2624/Vibrator.cpp
index 54a31cc..ad4ed34 100644
--- a/vibrator/drv2624/Vibrator.cpp
+++ b/vibrator/drv2624/Vibrator.cpp
@@ -70,6 +70,7 @@ static std::vector<float> sYAxleData;
 static uint64_t sEndTime = 0;
 static struct timespec sGetTime;
 
+#define MAX_VOLTAGE 3.2
 #define FLOAT_EPS 1e-7
 #define SENSOR_DATA_NUM 20
 // Set sensing period to 2s
@@ -158,7 +159,7 @@ static float targetGToVlevelsUnderLinearEquation(std::array<float, 4> inputCoeff
     // 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)) {
+    if ((outPutVal > FLOAT_EPS) && (outPutVal <= MAX_VOLTAGE)) {
         return outPutVal;
     } else {
         return 0.0f;
@@ -185,7 +186,7 @@ static float targetGToVlevelsUnderCubicEquation(std::array<float, 4> inputCoeffs
     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)) {
+        if ((outPutVal > FLOAT_EPS) && (outPutVal <= MAX_VOLTAGE)) {
             return outPutVal;
         }
         return 0.0f;
@@ -217,15 +218,15 @@ static float targetGToVlevelsUnderCubicEquation(std::array<float, 4> inputCoeffs
         sqrtA = sqrt(AA);
 
         outPutVal = (-inputCoeffs[1] - 2 * sqrtA * cosSita) / (3 * inputCoeffs[0]);
-        if ((outPutVal > FLOAT_EPS) && (outPutVal <= 3.2)) {
+        if ((outPutVal > FLOAT_EPS) && (outPutVal <= MAX_VOLTAGE)) {
             return outPutVal;
         }
         outPutVal = (-inputCoeffs[1] + sqrtA * (cosSita + sinSitaSqrt3)) / (3 * inputCoeffs[0]);
-        if ((outPutVal > FLOAT_EPS) && (outPutVal <= 3.2)) {
+        if ((outPutVal > FLOAT_EPS) && (outPutVal <= MAX_VOLTAGE)) {
             return outPutVal;
         }
         outPutVal = (-inputCoeffs[1] + sqrtA * (cosSita - sinSitaSqrt3)) / (3 * inputCoeffs[0]);
-        if ((outPutVal > FLOAT_EPS) && (outPutVal <= 3.2)) {
+        if ((outPutVal > FLOAT_EPS) && (outPutVal <= MAX_VOLTAGE)) {
             return outPutVal;
         }
         return 0.0f;
@@ -233,11 +234,11 @@ static float targetGToVlevelsUnderCubicEquation(std::array<float, 4> inputCoeffs
         // Case 4: Delta = 0
         K = BB / AA;
         outPutVal = (-inputCoeffs[1] / inputCoeffs[0] + K);
-        if ((outPutVal > FLOAT_EPS) && (outPutVal <= 3.2)) {
+        if ((outPutVal > FLOAT_EPS) && (outPutVal <= MAX_VOLTAGE)) {
             return outPutVal;
         }
         outPutVal = (-K / 2);
-        if ((outPutVal > FLOAT_EPS) && (outPutVal <= 3.2)) {
+        if ((outPutVal > FLOAT_EPS) && (outPutVal <= MAX_VOLTAGE)) {
             return outPutVal;
         }
         return 0.0f;
@@ -247,6 +248,13 @@ static float targetGToVlevelsUnderCubicEquation(std::array<float, 4> inputCoeffs
     }
 }
 
+static float vLevelsToTargetGUnderCubicEquation(std::array<float, 4> inputCoeffs, float vLevel) {
+    float inputVoltage = 0.0f;
+    inputVoltage = vLevel * MAX_VOLTAGE;
+    return inputCoeffs[0] * pow(inputVoltage, 3) + inputCoeffs[1] * pow(inputVoltage, 2) +
+           inputCoeffs[2] * inputVoltage + inputCoeffs[3];
+}
+
 static bool motionAwareness() {
     float avgX = 0.0, avgY = 0.0;
     uint64_t current_time = 0;
@@ -339,8 +347,21 @@ Vibrator::Vibrator(std::unique_ptr<HwApi> hwapi, std::unique_ptr<HwCal> hwcal)
         hasSteadyCoeffs = mHwCal->getSteadyCoeffs(&steadyCoeffs);
         if (hasSteadyCoeffs) {
             for (i = 0; i < 3; i++) {
-                // Use cubic approach to get the target voltage levels
-                tempVolLevel = targetGToVlevelsUnderCubicEquation(steadyCoeffs, STEADY_TARGET_G[i]);
+                // Use cubic approach to get the steady target voltage levels
+                // For steady level 3 voltage which is used for non-motion voltage, we use
+                // interpolation method to calculate the voltage via 20% of MAX
+                // voltage, 60% of MAX voltage and steady level 3 target G
+                if (i == 2) {
+                    tempVolLevel = ((STEADY_TARGET_G[2] -
+                                     vLevelsToTargetGUnderCubicEquation(steadyCoeffs, 0.2)) *
+                                    0.4 * MAX_VOLTAGE) /
+                                       (vLevelsToTargetGUnderCubicEquation(steadyCoeffs, 0.6) -
+                                        vLevelsToTargetGUnderCubicEquation(steadyCoeffs, 0.2)) +
+                                   0.2 * MAX_VOLTAGE;
+                } else {
+                    tempVolLevel =
+                        targetGToVlevelsUnderCubicEquation(steadyCoeffs, STEADY_TARGET_G[i]);
+                }
                 mSteadyTargetOdClamp[i] = convertLevelsToOdClamp(tempVolLevel, lraPeriod);
                 if ((mSteadyTargetOdClamp[i] <= 0) || (mSteadyTargetOdClamp[i] > longVoltageMax)) {
                     mSteadyTargetOdClamp[i] = longVoltageMax;