File content as of revision 39:80b38a8e1787:

#include <math.h>

#include "config_motor.h"
#include "Optimize.h"
#include "MathHelpers.h"
#include "optimize_constants.h"

void get_mtpa_dq(float tau, float *d, float *q) {
    float is = get_mtpa_is(tau);
    get_mtpa_is_dq(is, d, q);
}

void get_mtpf_dq(float tau, float w, float umax, float *d, float *q) {
    float x = umax / w;
    x = x * x;
    x *= cH;
    *d  = cF - cG - sqrtf(cH * cH + 0.5f * x);
    float y = cI - sqrtf(cI * cI + 0.5f * x);
    *q = x + y * y;
}

float get_mtpa_is(float tau) {
    float tau2 = tau * tau;
    float alpha = val_alpha(tau, tau2);
    float u = sqrtf(cA * tau2 / alpha + cB * alpha);
    float v = sqrtf(cC * tau2 / alpha + cD * alpha + cE * tau / u);
    float q = cU * u + cV * v;
    float d = (-3 * q * POLE_PAIRS * FLUX_LINKAGE + 2 * tau) / (3 * q * Lx * POLE_PAIRS);
    return sqrtf(d * d + q * q);
}

void get_mtpa_is_dq(float is, float *d, float *q) {
    *d = (-FLUX_LINKAGE + sqrtf(PSI2 + 8 * Lx * Lx * is * is)) / (4.f * Lx);
    *q = sqrtf(is * is - *d * *d);
}

float get_vs2(float w, float d, float q) {
    return w * w * (Lq * Lq * q * q + (Ld * d + FLUX_LINKAGE) * (Ld * d + FLUX_LINKAGE));
}

float val_alpha(float tau, float tau2) {
    float tau23 = acbrt(tau2);
    if (tau < TAU0) {
        return tau23 * K1_13 * (c0 + c2 * tau2 + c4 * tau2 * tau2);
    }
    else {
        return s0 + s1 * (tau - TAU0);
    }
}