stmbl

HAL_COMP(idq);

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

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

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

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


RT(
  float d = PIN(d);
  float q = PIN(q);

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

  //inverse park transformation
  float a = d * co - q * si;
  float b = d * si + q * co;
  
  //inverse clarke transformation
  float u =   a;
  float v = - a / 2.0 + b / 2.0 * M_SQRT3;
  float w = - a / 2.0 - b / 2.0 * M_SQRT3;

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

  PIN(u) = u;
  PIN(v) = v;
  PIN(w) = w;
);

ENDCOMP;