Diff: DQMapper/DQMapper.cpp

Revision:

171:3f1d1792757c

Parent:

161:19eac809c727

Child:

185:5c102874b490

--- a/DQMapper/DQMapper.cpp	Sat Dec 02 19:24:56 2017 +0000
+++ b/DQMapper/DQMapper.cpp	Sun Dec 03 01:28:51 2017 +0000
@@ -51,6 +51,70 @@
     }
 }
 
+float InterpolatingLutMapper::lookup(short table[][COLUMNS], int row, int col) {
+    if (row >= ROWS) row = ROWS - 1;
+    if (col >= COLUMNS) col = COLUMNS - 1;
+    return (float)table[row][col] / 128.f;
+}
+
+float InterpolatingLutMapper::lookup(short *table, int index) {
+    if (index >= ROWS) index = ROWS - 1;
+    return (float)table[index] / 128.f;
+}
+
+float InterpolatingLutMapper::interp(float a, float b, float eps) {
+    float delta = eps * (b - a);
+    return a + delta;
+}
+
+float InterpolatingLutMapper::interp(float a, float b, float c, float eps_row, float eps_col) {
+    float delta_row = eps_row * (b - a);
+    float delta_col = eps_col * (c - a);
+    return a + delta_row + delta_col;
+}
+
+void InterpolatingLutMapper::map(float torque_percent, float w, float *d, float *q) {
+    w *= POLE_PAIRS;
+    bool neg = false;
+
+    if (w < 0.f) {
+        w = -w;
+        torque_percent = -torque_percent;
+        neg = true;
+    }
+    if (fabs(torque_percent) < 0.02f) {
+        int index = (int)(w / W_STEP);
+        float eps = w / W_STEP - index;
+
+        *d = interp(lookup(zdtab, index), lookup(zdtab, index + 1), eps);
+        *q = interp(lookup(zqtab, index), lookup(zqtab, index + 1), eps);
+    }
+    else if (torque_percent >= 0.02f) {
+        int row = (int)(w / W_STEP);
+        int col = (int)((torque_percent - 0.02f) * COLUMNS);
+
+        float row_eps = w / W_STEP - row;
+        float col_eps = (torque_percent - 0.02f) * COLUMNS - col;
+
+        *d = interp(lookup(pdtab, row, col), lookup(pdtab, row + 1, col), lookup(pdtab, row, col + 1), row_eps, col_eps);
+        *q = interp(lookup(pqtab, row, col), lookup(pqtab, row + 1, col), lookup(pqtab, row, col + 1), row_eps, col_eps);
+    }
+    else {
+        int row = (int)(w / W_STEP);
+        int col = (int)((-torque_percent - 0.02f) * COLUMNS);
+
+        float row_eps = w / W_STEP - row;
+        float col_eps = (-torque_percent - 0.02f) * COLUMNS - col;
+
+        *d = interp(lookup(ndtab, row, col), lookup(ndtab, row + 1, col), lookup(ndtab, row, col + 1), row_eps, col_eps);
+        *q = interp(lookup(nqtab, row, col), lookup(nqtab, row + 1, col), lookup(nqtab, row, col + 1), row_eps, col_eps);
+    }
+
+    if (neg) {
+        *q = -*q;
+    }
+}
+
 void LinearNoFWMapper::map(float torque_percent, float w, float *d, float *q) {
     float is = torque_percent * _tmax / _kt;
     *d = (-_lambda + sqrtf(_lambda * _lambda + 8 * (Ld - Lq) * (Ld - Lq) * is * is)) / (4.f * (Ld - Lq));