stmbl

//using FB_TX

HAL_COMP(res);

HAL_PIN(pos) = 0.0;
HAL_PIN(amp) = 0.0;
HAL_PIN(quad) = 0.0;
HAL_PIN(poles) = 1.0;

HAL_PIN(vel) = 0.0; // TODO: vel rev, fb,cmd -> vel0,1 -> rev

HAL_PIN(sin) = 0.0;
HAL_PIN(cos) = 0.0;

HAL_PIN(enable) = 0.0;
HAL_PIN(error) = 0.0;
//HAL_PIN(tim2_oc) = 240.0;
MEM(int lastq) = 0;// last quadrant
MEM(int abspos) = 0;// multiturn position

RT_INIT(
   //resolver ref signal generation
   GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_10;
   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
   GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
   GPIO_Init(GPIOB, &GPIO_InitStructure);

   GPIO_PinAFConfig(GPIOB, GPIO_PinSource10, GPIO_AF_TIM2);

   // TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Toggle;
   // TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
   // TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable;
   // TIM_OCInitStructure.TIM_Pulse = 10;
   // TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
   // TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;
   // TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
   // TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Reset;
   // 
   // TIM_OC3Init(TIM2, &TIM_OCInitStructure);
   // TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);
   // TIM_CtrlPWMOutputs(TIM2, ENABLE);
   
   //txen
   GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_12;
   GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_OUT;
   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
   GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
   GPIO_Init(GPIOB, &GPIO_InitStructure);
   GPIO_SetBits(GPIOB,GPIO_Pin_12);
);

RT_DEINIT(
   GPIO_ResetBits(GPIOB,GPIO_Pin_12);//tx disable
   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_12;
   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
   GPIO_Init(GPIOB, &GPIO_InitStructure);
   // TIM_CtrlPWMOutputs(TIM2, DISABLE);
   // TIM_OCStructInit(&TIM_OCInitStructure);
   // TIM_OC3Init(TIM2, &TIM_OCInitStructure);
);

RT(
   //TODO: arr can change!
//   TIM2->CCR3 = (int)CLAMP(PIN(tim2_oc), 0, TIM2->ARR - 1);
	float s = 0.0;
	float c = 0.0;
	float a = 0.0;

	s = PIN(sin);
   c = PIN(cos);
	a = sqrtf(s * s + c * c);

	float p = MAX(1.0, PIN(poles));
   
   float pos = atan2f(s, c);
   float dpos = PIN(vel) * period / 2.0;

	if(a < 0.25){
		PIN(error) = 1.0;
	}
	else{
		PIN(error) = 0.0;
      if(p == 1.0f){
         PIN(pos) = mod(pos + dpos);
      }else{
         int q = PIN(quad);// current quadrant

         if(lastq == 2 && q == 3)
            abspos++;
         if(lastq == 3 && q == 2)
            abspos--;

			if(abspos >= p){
				abspos = 0;
			}
			if(abspos <= -1){
				abspos = p - 1;
			}

         lastq = q;
         //TODO: clamp abspos
         float absa = pos + abspos * M_PI * 2.0f;
         PIN(pos) = mod(absa/p + dpos);
      }
	}
	PIN(amp) = a;
);


ENDCOMP;