stmbl

HAL_COMP(dq);

//U V W inputs
HAL_PIN(u) = 0.0;
HAL_PIN(v) = 0.0;
HAL_PIN(w) = 0.0;

//rotor position
HAL_PIN(pos) = 0.0;
HAL_PIN(polecount) = 1.0;

//a,b,gamma output
HAL_PIN(a) = 0.0;
HAL_PIN(b) = 0.0;
HAL_PIN(y) = 0.0;

//d,q output
HAL_PIN(d) = 0.0;
HAL_PIN(q) = 0.0;

RT(
   float u = PIN(u);
   float v = PIN(v);
   float w = PIN(w);
   
   //clarke transformation
   float a = u * 2.0 / 3.0 - v / 3.0 - w / 3.0;
   float b = v / M_SQRT3 - w / M_SQRT3;
   float y = u / 3.0 + v / 3.0 + w / 3.0;

   float p = (int)MAX(PIN(polecount), 1.0);
   float pos = PIN(pos) * p;

   float si = 0.0;
   float co = 0.0;
   sincos_fast(pos, &si, &co);

   //park transformation
   float d =   a * co + b * si;
   float q = - a * si + b * co;

   PIN(a) = a;
   PIN(b) = b;
   PIN(y) = y;

   PIN(d) = d;
   PIN(q) = q;
);

ENDCOMP;