DQMapper.cpp

00001 #include "math.h"
00002 
00003 #include "DQMapper.h"
00004 #include "prefs.h"
00005 
00006 #include "tables.h"
00007 #include "ndtab.h"
00008 #include "nqtab.h"
00009 #include "pdtab.h"
00010 #include "pqtab.h"
00011 #include "zdtab.h"
00012 #include "zqtab.h"
00013 
00014 void LutMapper::map(float torque_percent, float w, float *d, float *q) {
00015     w *= _POLE_PAIRS;
00016     bool neg = false;
00017     
00018     if (w < 0.f) {
00019         w = -w;
00020         torque_percent = -torque_percent;
00021         neg = true;
00022     }
00023     if (fabs(torque_percent) < 0.02f) {
00024         int index = (int) (w / _W_STEP);
00025         if (index >= ROWS) index = ROWS - 1;
00026         
00027         *d = (float) zdtab[index] / 128.f;
00028         *q = (float) zqtab[index] / 128.f;
00029     } else if (torque_percent >= 0.02f) {
00030         int row = (int) (w / _W_STEP);
00031         int col = (int) ((torque_percent - 0.02f) * COLUMNS);
00032         
00033         if (row >= ROWS) row = ROWS - 1;
00034         if (col >= COLUMNS) col = COLUMNS - 1;
00035         
00036         *d = (float) pdtab[row][col] / 128.f;
00037         *q = (float) pqtab[row][col] / 128.f;
00038     } else {
00039         int row = (int) (w / _W_STEP);
00040         int col = (int) ((-torque_percent - 0.02f) * COLUMNS);
00041         
00042         if (row >= ROWS) row = ROWS - 1;
00043         if (col >= COLUMNS) col = COLUMNS - 1;
00044         
00045         *d = (float) ndtab[row][col] / 128.f;
00046         *q = (float) nqtab[row][col] / 128.f;
00047     }
00048     
00049     if (neg) {
00050         *q = -*q;
00051     }
00052 }
00053 
00054 float InterpolatingLutMapper::lookup(short table[][COLUMNS], int row, int col) {
00055     if (row >= ROWS) row = ROWS - 1;
00056     if (col >= COLUMNS) col = COLUMNS - 1;
00057     return (float)table[row][col] / 128.f;
00058 }
00059 
00060 float InterpolatingLutMapper::lookup(short *table, int index) {
00061     if (index >= ROWS) index = ROWS - 1;
00062     return (float)table[index] / 128.f;
00063 }
00064 
00065 float InterpolatingLutMapper::interp(float a, float b, float eps) {
00066     float delta = eps * (b - a);
00067     return a + delta;
00068 }
00069 
00070 float InterpolatingLutMapper::interp(float a, float b, float c, float eps_row, float eps_col) {
00071     float delta_row = eps_row * (b - a);
00072     float delta_col = eps_col * (c - a);
00073     return a + delta_row + delta_col;
00074 }
00075 
00076 void InterpolatingLutMapper::map(float torque_percent, float w, float *d, float *q) {
00077     w *= _POLE_PAIRS;
00078     bool neg = false;
00079 
00080     if (w < 0.f) {
00081         w = -w;
00082         torque_percent = -torque_percent;
00083         neg = true;
00084     }
00085     if (fabs(torque_percent) < 0.02f) {
00086         int index = (int)(w / _W_STEP);
00087         float eps = w / _W_STEP - index;
00088 
00089         *d = interp(lookup(zdtab, index), lookup(zdtab, index + 1), eps);
00090         *q = interp(lookup(zqtab, index), lookup(zqtab, index + 1), eps);
00091     }
00092     else if (torque_percent >= 0.02f) {
00093         int row = (int)(w / _W_STEP);
00094         int col = (int)((torque_percent - 0.02f) * COLUMNS);
00095 
00096         float row_eps = w / _W_STEP - row;
00097         float col_eps = (torque_percent - 0.02f) * COLUMNS - col;
00098 
00099         *d = interp(lookup(pdtab, row, col), lookup(pdtab, row + 1, col), lookup(pdtab, row, col + 1), row_eps, col_eps);
00100         *q = interp(lookup(pqtab, row, col), lookup(pqtab, row + 1, col), lookup(pqtab, row, col + 1), row_eps, col_eps);
00101     }
00102     else {
00103         int row = (int)(w / _W_STEP);
00104         int col = (int)((-torque_percent - 0.02f) * COLUMNS);
00105 
00106         float row_eps = w / _W_STEP - row;
00107         float col_eps = (-torque_percent - 0.02f) * COLUMNS - col;
00108 
00109         *d = interp(lookup(ndtab, row, col), lookup(ndtab, row + 1, col), lookup(ndtab, row, col + 1), row_eps, col_eps);
00110         *q = interp(lookup(nqtab, row, col), lookup(nqtab, row + 1, col), lookup(nqtab, row, col + 1), row_eps, col_eps);
00111     }
00112 
00113     if (neg) {
00114         *q = -*q;
00115     }
00116 }
00117 
00118 void LinearNoFWMapper::map(float torque_percent, float w, float *d, float *q) {
00119     float is = torque_percent * _tmax / _kt;
00120     *d = (-_lambda + sqrtf(_lambda * _lambda + 8 * (_Ld - _Lq) * (_Ld - _Lq) * is * is)) / (4.f * (_Ld - _Lq));
00121     *q = sqrtf(is * is - *d * *d);
00122 }
00123 
00124 void AngleMapper::map(float torque_percent, float w, float *d, float *q) {
00125     *q = _is * torque_percent * sinf(_theta);
00126     *d = _is * torque_percent * cosf(_theta);
00127 }
00128 
00129 void DirectMapper::map(float torque_percent, float w, float *d, float *q) {
00130     *d = _id * torque_percent;
00131     *q = _iq * torque_percent;
00132 }
00133 
00134 void SwapMapper::map(float torque_percent, float w, float *d, float *q) {
00135     if (torque_percent < 0.5f) {
00136         *d = 0.0f;
00137         *q = _iq;
00138     } else {
00139         *d = _id;
00140         *q = 0.0f;
00141     }
00142 }
00143 
00144 void AutoMapper::map(float torque_percent, float w, float *d, float *q) {
00145     if (torque_percent > 0.99f) {
00146         _theta += (_phase_high - _phase_low) / _steps;
00147         torque_percent = 0.0f;
00148     }
00149     
00150     if (_theta >= _phase_high) {
00151         *q = 0.0f;
00152         *d = 0.0f;
00153         return;
00154     }
00155     
00156     *q = _is * torque_percent * sinf(_theta);
00157     *d = _is * torque_percent * cosf(_theta);
00158 }