File content as of revision 252:38644631ed97:

#include "math.h"

#include "DQMapper.h"
#include "prefs.h"

#include "tables.h"
#include "ndtab.h"
#include "nqtab.h"
#include "pdtab.h"
#include "pqtab.h"
#include "zdtab.h"
#include "zqtab.h"

void LutMapper::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);
        if (index >= ROWS) index = ROWS - 1;
        
        *d = (float) zdtab[index] / 128.f;
        *q = (float) zqtab[index] / 128.f;
    } else if (torque_percent >= 0.02f) {
        int row = (int) (w / _W_STEP);
        int col = (int) ((torque_percent - 0.02f) * COLUMNS);
        
        if (row >= ROWS) row = ROWS - 1;
        if (col >= COLUMNS) col = COLUMNS - 1;
        
        *d = (float) pdtab[row][col] / 128.f;
        *q = (float) pqtab[row][col] / 128.f;
    } else {
        int row = (int) (w / _W_STEP);
        int col = (int) ((-torque_percent - 0.02f) * COLUMNS);
        
        if (row >= ROWS) row = ROWS - 1;
        if (col >= COLUMNS) col = COLUMNS - 1;
        
        *d = (float) ndtab[row][col] / 128.f;
        *q = (float) nqtab[row][col] / 128.f;
    }
    
    if (neg) {
        *q = -*q;
    }
}

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));
    *q = sqrtf(is * is - *d * *d);
}

void AngleMapper::map(float torque_percent, float w, float *d, float *q) {
    *q = _is * torque_percent * sinf(_theta);
    *d = _is * torque_percent * cosf(_theta);
}

void DirectMapper::map(float torque_percent, float w, float *d, float *q) {
    *d = _id * torque_percent;
    *q = _iq * torque_percent;
}

void SwapMapper::map(float torque_percent, float w, float *d, float *q) {
    if (torque_percent < 0.5f) {
        *d = 0.0f;
        *q = _iq;
    } else {
        *d = _id;
        *q = 0.0f;
    }
}

void AutoMapper::map(float torque_percent, float w, float *d, float *q) {
    if (torque_percent > 0.99f) {
        _theta += (_phase_high - _phase_low) / _steps;
        torque_percent = 0.0f;
    }
    
    if (_theta >= _phase_high) {
        *q = 0.0f;
        *d = 0.0f;
        return;
    }
    
    *q = _is * torque_percent * sinf(_theta);
    *d = _is * torque_percent * cosf(_theta);
}