File content as of revision 32:b31423041c4e:

#include <math.h>

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

void get_mtpa_dq(float tau, float *d, float *q) {
    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);
    *q = cU * u + cV * v;
    *d = (-3 * *q * POLE_PAIRS * FLUX_LINKAGE + 2 * tau) / (3 * *q * Lx * POLE_PAIRS);
}

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

}

float acbrt(float x0) {
    union { int ix; float x; };

    x = x0;                      // x can be viewed as int.
    ix = 0x2a5137a0 + ix / 3;        // Initial guess.  
    x = 0.33333333f*(2.0f*x + x0 / (x * x));  // Newton step.

    return x;
}