mc_stm8s_bldc_drive.c

STM8S105 BLDC源代码

	#endif

	//disable channel outputs
	TIM1->CCER1 = (A_OFF|B_OFF);
	//disable channel outputs
	TIM1->CCER2 = C_OFF;
	
	//enable main outputs, enable break, OSSR=1, OSSI=1, LOCKLEVEL 2, Break Polarity
	#define TIM1_OSSRSTATE_ENABLE BIT3
	#ifndef LS_GPIO_CONTROL
		TIM1->BKR = (DEV_BKIN_POLARITY|DEV_BKIN|TIM1_OSSRSTATE_ENABLE|TIM1_OSSISTATE_ENABLE|TIM1_LOCKLEVEL_2); 
	#else
		// If GPIO is used to drive low side then the disabled outputs of the timer
		// must be not driven by the timer during run time (MOE = 1)
		TIM1->BKR = (DEV_BKIN_POLARITY|DEV_BKIN|TIM1_OSSISTATE_ENABLE|TIM1_LOCKLEVEL_2); 
	#endif

	//counter enabled
	TIM1->CR1 |= BIT0;

	//force timer update
	TIM1->EGR = (BIT5|BIT0);

	//enable break interrupt
	TIM1->IER = BIT7;

	#ifdef ETR_INPUT
		//Enable ETR pin
		GPIO_Init(ETR_PORT, ETR_PIN,GPIO_MODE_IN_FL_NO_IT);
	#endif
}

void Init_TIM2(void)
{
#ifdef SENSORLESS
	//counter disabled, ARR preload register disabled, up counting, edge aligned mode
	TIM2->CR1 = BIT2;

	//disable all interrupts
	TIM2->IER = 0;

	TIM2->CCER1 = 0;
	TIM2->CCER2 = 0;

	//select PWM mode 2, OC2 preload enabled
	TIM2->CCMR2 = BIT6|BIT5|BIT3;

	//prescale = div3 @ 16MHz -> 0.5us/count * 24MHz -> 0.33us/count
	TIM2->PSCR = 0; 

	ToCMPxH( TIM2->ARRH, ARRTIM2 );
	ToCMPxL( TIM2->ARRL, ARRTIM2 );

	// Current Limitation
	ToCMPxH( TIM2->CCR2H, MILLIAMP_TOCNT (STARTUP_CURRENT_LIMITATION) );
	ToCMPxL( TIM2->CCR2L, MILLIAMP_TOCNT (STARTUP_CURRENT_LIMITATION) );		 

	//Enable output
	TIM2->CCER1 |= BIT4;

	//counter enabled
	TIM2->CR1 |= BIT0;

	//force timer update
	TIM2->EGR = (BIT5|BIT0);
#endif
#ifdef HALL
	// Configure the TIM2 to manage the Hall sensorssignals
	// Set channel 1,2 and 3 as input capture

	//counter disabled, ARR preload register disabled, up counting, edge aligned mode
	TIM2->CR1 = BIT2;

	//disable all interrupts
	TIM2->IER = 0;

	TIM2->CCER1 = 0;
	TIM2->CCER2 = 0;

	//select channel 1,2 and 3 as input, channel prescaler 0
	TIM2->CCMR1 = BIT0;
	TIM2->CCMR2 = BIT0;
	TIM2->CCMR3 = BIT0;

	#define IC_FILTER (u8)(HALL_FILTER << 4)

	TIM2->CCMR1 |= IC_FILTER;
	TIM2->CCMR2 |= IC_FILTER;
	TIM2->CCMR3 |= IC_FILTER;

	//prescale = div3 @ 16MHz -> 0.5us/count * 24MHz -> 0.33us/count
	TIM2->PSCR = 0; 

	ToCMPxH( TIM2->ARRH, 0xFFFF );
	ToCMPxL( TIM2->ARRL, 0xFFFF );

	//Enable capture
	TIM2->CCER1 |= BIT0;
	TIM2->CCER1 |= BIT4;
	TIM2->CCER2 |= BIT0;

	//counter enabled
	TIM2->CR1 |= BIT0;
#endif
}

#ifdef HALL
	void TIM2_InitCapturePolarity(void)
	{
		u8 bHStatus = 0;
		GPIOD->DDR &= (u8)(~(BIT3|BIT4));
		GPIOA->DDR &= (u8)(~(BIT3));
		
		// Read status of H1 and set the expected polarity
		if (H1_PORT & H1_PIN)
		{
			TIM2->CCER1 |= BIT5;
			bHStatus |= BIT2;
		}
		else
		{
			TIM2->CCER1 &= (u8)(~(BIT5));
		}
		
		// Read status of H2 and set the expected polarity
		if (H2_PORT & H2_PIN)
		{
			TIM2->CCER1 |= BIT1;
			bHStatus |= BIT1;
		}
		else
		{
			TIM2->CCER1 &= (u8)(~(BIT1));
		}
		
		// Read status of H3 and set the expected polarity
		if (H3_PORT & H3_PIN)
		{
			TIM2->CCER2 |= BIT1;
			bHStatus |= BIT0;
		}
		else
		{
			TIM2->CCER2 &= (u8)(~(BIT1));
		}

		bHallStartStep = bHallSteps[bHStatus];

		if (bHallStartStep == NOT_VALID)
		{
			MTC_Status |= MTC_STARTUP_FAILED;
		}
		
		TIM2_ClearITPendingBit(TIM2_IT_CC1);
		TIM2_ClearITPendingBit(TIM2_IT_CC2);
		TIM2_ClearITPendingBit(TIM2_IT_CC3);
		
		// Enable Input Capture Interrupt
		TIM2->IER = BIT1|BIT2|BIT3;
	}
#endif

#ifdef HALL
	void Hall_Timeout(void)
	{
		// Set zero speed
		*pcounter_reg = 0;
		first_cap = 0;
	}
#endif

#ifdef TIMER2_HANDLES_HALL
@near @interrupt @svlreg void TIM2_CAP_COM_IRQHandler (void)
{
	u8 bHStatus = 0;

	#ifdef DEBUG_PINS
		Z_DEBUG_PORT ^= Z_DEBUG_PIN;
	#endif

	GetStepTime();

	// Read status of H1 and set the expected polarity
	if (H1_PORT & H1_PIN)
	{
		TIM2->CCER1 |= BIT5;   //up polarity ///
		bHStatus |= BIT2;
	}
	else
	{
		TIM2->CCER1 &= (u8)(~(BIT5));  //down polarity  ///
	}
	
	// Read status of H2 and set the expected polarity
	if (H2_PORT & H2_PIN)
	{
		TIM2->CCER1 |= BIT1;
		bHStatus |= BIT1;
	}
	else
	{
		TIM2->CCER1 &= (u8)(~(BIT1));
	}
	
	// Read status of H3 and set the expected polarity
	if (H3_PORT & H3_PIN)
	{
		TIM2->CCER2 |= BIT1;
		bHStatus |= BIT0;
	}
	else
	{
		TIM2->CCER2 &= (u8)(~(BIT1));
	}
	
	if (TIM2->SR1 & BIT2)
	{
		TIM2_ClearITPendingBit(TIM2_IT_CC2);
	}

	if (TIM2->SR1 & BIT1)
	{
		TIM2_ClearITPendingBit(TIM2_IT_CC1);
	}

	if (TIM2->SR1 & BIT3)
	{
		TIM2_ClearITPendingBit(TIM2_IT_CC3);
	}

	Current_Step = (u8)(bHallSteps[bHStatus]);
	if (Current_Step == NOT_VALID)
	{
		MTC_Status |= MTC_MOTOR_STALLED;
	}

	Current_BEMF = BEMFSteps[Current_Step].BEMF_Level;
	
	TIM1->CCMR1 = PhaseSteps[Current_Step].CCMR_1;
	TIM1->CCMR2 = PhaseSteps[Current_Step].CCMR_2;
	TIM1->CCMR3 = PhaseSteps[Current_Step].CCMR_3;
	TIM1->CCER1 = PhaseSteps[Current_Step].CCER_1;
	TIM1->CCER2 = PhaseSteps[Current_Step].CCER_2;
	
	Phase_State = PHASE_ZERO;
	ComHandler();

	// Timeout refresh
	vtimer_SetTimer(HALL_CAPT_TIMEOUT_TIMER,HALL_CAPT_TIMEOUT_MS,&Hall_Timeout);

	SpeedMeasurement();
	
	return;
}
#endif

void ComHandler(void)
{
	u16 cur_time;
	u16 temp_time;
	u8 i;

	switch( Phase_State )
	{
	case PHASE_COMM:
		#ifdef DEBUG_PINS
			C_D_DEBUG_PORT |= C_D_DEBUG_PIN;
		#endif
		Commutate_Motor();
		#ifdef SENSORLESS
			Phase_State = PHASE_DEMAG; 
		#endif	
		#ifdef HALL
			Phase_State = PHASE_ZERO;
		#endif
//		Enable_ADC_User_Sync_Sampling();
	break;

	case PHASE_DEMAG:
		#ifdef DEBUG_PINS
			C_D_DEBUG_PORT &= (u8)(~C_D_DEBUG_PIN);
		#endif
		if( (MTC_Status & MTC_STEP_MODE) == MTC_STEP_MODE )
		{
			//load commutation time from ramp table
			Commutation_Time = RAMP_TABLE[ Ramp_Step ];
			if( Ramp_Step < STEP_RAMP_SIZE )
			{
				Ramp_Step++;
			}
			else
			{
				MTC_Status |= MTC_STARTUP_FAILED;                  
			}

			//load commutation time into timer
			hTim3Th = LastSwitchedCom + Commutation_Time;
		}
		else
		{
			//load fail safe commutation time in case zero cross not detected (2 zero crossing time)
			temp_time = Previous_Zero_Cross_Time * 2;
			hTim3Th = hTim3Cnt + temp_time;
		}

		//commutate the motor
		TIM1->EGR |= BIT5;

		// Update CCMRx OCxCE bit (Actual)
		TIM1->CCMR1 = (u8)(PhaseSteps[Current_Step].CCMR_1 & 0x80);
		TIM1->CCMR2 = (u8)(PhaseSteps[Current_Step].CCMR_2 & 0x80);
		TIM1->CCMR3 = (u8)(PhaseSteps[Current_Step].CCMR_3 & 0x80);

		//preload next step
		Current_Step++;
		if( Current_Step >= NUMBER_PHASE_STEPS )
		{
			Current_Step = 0;
		}
	
		if (g_pBLDC_Struct->pBLDC_Var->bFastDemag == 1)
		{
			// Set fast demag
			TIM1->CCMR1 |= (u8)(Fast_Demag_Steps[Current_Step].CCMR_1 & 0x7F);
			TIM1->CCMR2 |= (u8)(Fast_Demag_Steps[Current_Step].CCMR_2 & 0x7F);
			TIM1->CCMR3 |= (u8)(Fast_Demag_Steps[Current_Step].CCMR_3 & 0x7F);
			tmp_TIM1_CCER1 = Fast_Demag_Steps[Current_Step].CCER_1;
			tmp_TIM1_CCER2 = Fast_Demag_Steps[Current_Step].CCER_2;
		}
		else
		{
			// Set fast demag
			TIM1->CCMR1 |= (u8)(PhaseSteps[Current_Step].CCMR_1 & 0x7F);
			TIM1->CCMR2 |= (u8)(PhaseSteps[Current_Step].CCMR_2 & 0x7F);
			TIM1->CCMR3 |= (u8)(PhaseSteps[Current_Step].CCMR_3 & 0x7F);
			tmp_TIM1_CCER1 = PhaseSteps[Current_Step].CCER_1;
			tmp_TIM1_CCER2 = PhaseSteps[Current_Step].CCER_2;
		}

		// Store the values
		tmp_TIM1_CCMR1 = TIM1->CCMR1;
		tmp_TIM1_CCMR2 = TIM1->CCMR2;
		tmp_TIM1_CCMR3 = TIM1->CCMR3;

		// Set to frozen for next COM
		TIM1->CCMR1 &= 0x8F;
		TIM1->CCMR2 &= 0x8F;
		TIM1->CCMR3 &= 0x8F;

		Enable_ADC_BEMF_Sampling();
		Phase_State = PHASE_ZERO; 
		break;

	case PHASE_ZERO:
		Enable_ADC_Current_Sampling();
		if( Zero_Cross_Count != Last_Zero_Cross_Count )
		{
			#ifdef HALL
				#ifdef DEBUG_PINS
					C_D_DEBUG_PORT &= (u8)(~C_D_DEBUG_PIN);
				#endif
			#endif
			
			if( (MTC_Status & MTC_STEP_MODE) != MTC_STEP_MODE )
			{
				// Autoswitch mode
				
				//adjust commutation time based on motor speed

				if( Current_BEMF == BEMF_FALLING )
				{
					#asm
						; Commutation_Time = (Previous_Zero_Cross_Time * BEMF_Falling_Factor) >> 8;
						
						ld A,_Previous_Zero_Cross_Time
						ld XL,A
						ld A,_BEMF_Falling_Factor
						mul X,A
						ldw _Commutation_Time,X
						ld A,_Previous_Zero_Cross_Time + 1
						ld XL,A
						ld A,_BEMF_Falling_Factor
						mul X,A
						ld A,XH
						clrw X
						ld XL,A
						addw X,_Commutation_Time
						ldw _Commutation_Time,X
					#endasm
				}
				else
				{
					Motor_Stall_Count = 0;
					#asm
						; Commutation_Time = (Previous_Zero_Cross_Time * BEMF_Rising_Factor) >> 8;
						
						ld A,_Previous_Zero_Cross_Time
						ld XL,A
						ld A,_BEMF_Rising_Factor
						mul X,A
						ldw _Commutation_Time,X
						ld A,_Previous_Zero_Cross_Time + 1
						ld XL,A
						ld A,_BEMF_Rising_Factor
						mul X,A
						ld A,XH
						clrw X
						ld XL,A
						addw X,_Commutation_Time
						ldw _Commutation_Time,X
					#endasm
				}

				if( Zero_Sample_Count == 1 )
				{                                      

					Commutation_Time >>= 1;
				}
				
				hTim3Th = Commutation_Time;
			}
			else
			{
				#ifdef SENSORLESS
					//if number of consectutive BEMF events occurred, switch out of stepped mode
					if( Zero_Cross_Count >= MINIMUM_CONSECTIVE_ZERO_CROSS )
					{
						MTC_Status |= MTC_LAST_FORCED_STEP;
						Motor_Stall_Count = 0;
						Commutation_Time = Average_Zero_Cross_Time;
						
						hTim3Th = Average_Zero_Cross_Time;
		
						//load commutation time into timer
						Previous_Zero_Cross_Time = Commutation_Time;
						Zero_Cross_Time = Commutation_Time;
					}
				#endif
				#ifdef HALL
					MTC_Status &= (u8)(~MTC_STEP_MODE);
					
					// Force the commutation after first edge
					Phase_State = PHASE_COMM;
					ComHandler();
				#endif
			}
			
			Average_Zero_Cross_Time = (Previous_Zero_Cross_Time + Zero_Cross_Time) >> 1;
			Previous_Zero_Cross_Time = Zero_Cross_Time;
			Phase_State = PHASE_COMM; 
		}
		else
		{
		#ifdef SENSORLESS
			//if zero crossing not detected - commutate motor
			if( (MTC_Status & MTC_STEP_MODE) != MTC_STEP_MODE )
			{
				if( Current_BEMF == BEMF_RISING )
				{
					if( Motor_Stall_Count < MOTOR_STALL_THRESHOLD )
					{
						Motor_Stall_Count++;
					}
					else
					{
						MTC_Status |= MTC_MOTOR_STALLED;
					}
				}
			}  
			#ifdef DEBUG_PINS
				C_D_DEBUG_PORT |= C_D_DEBUG_PIN;
			#endif
			Commutate_Motor();
			Zero_Cross_Count=0;
			#ifdef SENSORLESS
				Phase_State = PHASE_DEMAG; 
			#endif
			#ifdef HALL
				Phase_State = PHASE_ZERO;
			#endif
		#endif
		}
		Last_Zero_Cross_Count = Zero_Cross_Count;
		break;

	default:
		Phase_State = PHASE_COMM; 
		break;
	}
}
	
#ifdef SENSORLESS
	void Commutate_Motor( void )
	{
		u16 cur_time;

		cur_time = hTim3Cnt;

		// Switch to Frozen
		TIM1->EGR |= BIT5;

		// Restore Values
		TIM1->CCMR1 = tmp_TIM1_CCMR1;
		TIM1->CCMR2 = tmp_TIM1_CCMR2;
		TIM1->CCMR3 = tmp_TIM1_CCMR3;
		TIM1->CCER1 = tmp_TIM1_CCER1;
		TIM1->CCER2 = tmp_TIM1_CCER2;

		//commutate the motor
		TIM1->EGR |= BIT5;
		
		#ifdef LS_GPIO_CONTROL
		{
			LS_Conf = LS_Steps_SW[Current_Step];

			if (LS_Conf == LS_A_B)
			{
				// Activate   B
				#if (PWM_V_LOW_SIDE_POLARITY == ACTIVE_HIGH)
					LS_B_PORT->ODR |= LS_B_PIN;
				#else
					LS_B_PORT->ODR &= (u8)(~LS_B_PIN);
				#endif
				// Deactivate A
				#if (PWM_U_LOW_SIDE_POLARITY == ACTIVE_HIGH)
					LS_A_PORT->ODR &= (u8)(~LS_A_PIN);
				#else
					LS_A_PORT->ODR |= LS_A_PIN;
				#endif
			}
			
			if (LS_Conf == LS_A_C)
			{
				// Activate   C
				#if (PWM_W_LOW_SIDE_POLARITY == ACTIVE_HIGH)
					LS_C_PORT->ODR |= LS_C_PIN;
				#else
					LS_C_PORT->ODR &= (u8)(~LS_C_PIN);
				#endif
				// Deactivate A
				#if (PWM_U_LOW_SIDE_POLARITY == ACTIVE_HIGH)
					LS_A_PORT->ODR &= (u8)(~LS_A_PIN);
				#else
					LS_A_PORT->ODR |= LS_A_PIN;
				#endif
			}

			if (LS_Conf == LS_B_A)
			{
				// Activate   A
				#if (PWM_U_LOW_SIDE_POLARITY == ACTIVE_HIGH)
					LS_A_PORT->ODR |= LS_A_PIN;