File content as of revision 45:cf8ad81fb0f0:

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

#include "config_motor.h"
#include "config_table.h"

#include "dtab_n_w25.h"
#include "dtab_p_w25.h"
#include "qtab_n_w25.h"
#include "qtab_p_w25.h"

void LutMapper::map(float torque_percent, float w, float *d, float *q) {
    w *= POLE_PAIRS;
    if (w < 0.f) {
        w = -w;
        torque_percent = -torque_percent;
    }
    if (torque_percent > 0) {
        int row = (int) (w / W_STEP);
        int col = (int) (torque_percent * COLUMNS);
        
        if (row >= ROWS) row = ROWS - 1;
        if (col >= COLUMNS) col = COLUMNS - 1;
        
        *d = (float) dtab_p_w25[row][col] / 128.f;
        *q = (float) qtab_p_w25[row][col] / 128.f; 
    } else {
        int row = (int) (w / W_STEP);
        int col = (int) (-torque_percent * COLUMNS);
        
        if (row >= ROWS) row = ROWS - 1;
        if (col >= COLUMNS) col = COLUMNS - 1;
        
        *d = (float) dtab_n_w25[row][col] / 128.f;
        *q = (float) qtab_n_w25[row][col] / 128.f;
    }
}

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