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(error_n) = 1.0;
HAL_PIN(tim_oc) = 47.0;
MEM(int lastq) = 0;// last quadrant
MEM(int abspos) = 0;// multiturn position

// TODO: in hal stop, reset adc dma

RT_INIT(
   //timer init for v4, v3 uses slave timer
   RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
   TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
   TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
   TIM_TimeBaseStructure.TIM_Period = 60 - 1;// 1.2e6 / 60 = 20kHz
   TIM_TimeBaseStructure.TIM_Prescaler = 0;
   TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
   TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
   TIM_SelectSlaveMode(TIM4, TIM_SlaveMode_External1);// Rising edges of the selected trigger (TRGI) clock the counter
   TIM_ITRxExternalClockConfig(TIM4, TIM_TS_ITR2);// clk = TIM_MASTER(TIM2) trigger out
   TIM_ARRPreloadConfig(TIM4,ENABLE);
   TIM_Cmd(TIM4, ENABLE);
   
   // resolver reference signal OC
   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 = 0;
   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;
   //ref is always OC3
   TIM_OC3Init(TIM4, &TIM_OCInitStructure);
   TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable);
   TIM_CtrlPWMOutputs(TIM4, ENABLE);
   
   //resolver ref signal generation
   GPIO_InitStructure.GPIO_Pin   = FB0_Z_PIN;
   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(FB0_Z_PORT, &GPIO_InitStructure);
   GPIO_PinAFConfig(FB0_Z_PORT, FB0_Z_PIN_SOURCE, GPIO_AF_TIM4);

   // 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   = FB0_Z_TXEN_PIN;
   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(FB0_Z_TXEN_PORT, &GPIO_InitStructure);
   GPIO_SetBits(FB0_Z_TXEN_PORT,FB0_Z_TXEN_PIN);
);

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!
   TIM4->CCR3 = (int)CLAMP(PIN(tim_oc), 0, TIM4->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;
    PIN(error_n) = 0.0;
	}
	else{
		PIN(error) = 0.0;
    PIN(error_n) = 1.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;