File content as of revision 79:d0b1bb3dcf68:

#include "math.h"
#include "DQMapper.h"

#include "config_motor.h"
#include "config_table.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;
    }
}

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);
}