mc_stm8s_bldc_drive.c

STM8S105 BLDC源代码

	#ifdef SET_TARGET_SPEED_BY_POTENTIOMETER
		// Set Target speed
		
		BLDC_Set_Target_rotor_speed((u16)(((u32)MAX_SPEED_RPM * hAsyncUserAdc)/1023));
		if (BLDC_Get_Target_rotor_speed() < MIN_SPEED_01HZ)
		  BLDC_Set_Target_rotor_speed(MIN_SPEED_01HZ); 
	#else
		// Call User defined function with hAsyncUserAdc parameter
	#endif
}

void Init_ADC( void )
{
	u8 value;
	u16 ADC_TDR_tmp;

	ADC_Sync_State = ADC_USER_SYNC_INIT;
	ADC_State = ADC_SYNC;

	ADC2->CSR = 0; 
	
	//select 4MHz clock based on 16MHz fMaster (div4), single mode
	//       6MHz clock based on 24MHz fMaster (div4)
	ADC2->CR1 = BIT5;

	//select internal trigger TIM1 TRGO
	ADC2->CR2 = 0;

	//select phase input
	ADC2->CSR = PHASE_C_BEMF_ADC_CHAN;

	ADC_TDR_tmp = 0;
	ADC_TDR_tmp |= (u16)(1) << PHASE_A_BEMF_ADC_CHAN;
	ADC_TDR_tmp |= (u16)(1) << PHASE_B_BEMF_ADC_CHAN;
	ADC_TDR_tmp |= (u16)(1) << PHASE_C_BEMF_ADC_CHAN;

	ADC_TDR_tmp |= (u16)(1) << ADC_CURRENT_CHANNEL;
	ADC_TDR_tmp |= (u16)(1) << ADC_USER_SYNC_CHANNEL;

	ADC_TDR_tmp |= (u16)(1) << ADC_BUS_CHANNEL;
	ADC_TDR_tmp |= (u16)(1) << ADC_NEUTRAL_POINT_CHANNEL;
	ADC_TDR_tmp |= (u16)(1) << ADC_TEMP_CHANNEL;
	ADC_TDR_tmp |= (u16)(1) << ADC_USER_ASYNC_CHANNEL;

	ToCMPxH( ADC2->TDRH, ADC_TDR_tmp);
	ToCMPxL( ADC2->TDRL, ADC_TDR_tmp);
	
	//enable trigger
	ADC2->CR2 |= BIT6;

	//enable ADC
	ADC2->CR1 |= BIT0;
	//allow ADC to stabilize
	value=30;
	while(value--);  
	ADC2->CR1 |= BIT0;    /////////////////
	//clear interrupt flag
	ADC2->CSR &= (u8)(~BIT7);
	ADC2->CSR |= BIT5;
	
}


void Enable_ADC_BEMF_Sampling( void )
{
	//Enable sampling of BEMF
	ADC_Sync_State = ADC_BEMF_INIT;
}

void Enable_ADC_Current_Sampling( void )
{
	//Enable sampling of the current
	ADC_Sync_State = ADC_CURRENT_INIT;
}

void Enable_ADC_User_Sync_Sampling( void )
{
	//allow application use of ADC
	ADC_Sync_State = ADC_USER_SYNC_INIT;
}

// This function capture the time into the Zero_Cross_Time vars
// express in number of PWM period occured from the last capture
void GetStepTime(void)
{
	u16 cur_time;

	cur_time = hTim3Cnt;
	// Reset counter
	hTim3Cnt = 0;

	Zero_Cross_Time = cur_time;
	Zero_Cross_Count++;
}

// This function manage the speed measurement using a buffer
void SpeedMeasurement(void)
{
	if (first_cap == 0)
	{
		first_cap = 1;
		cap_val = 0;
		cap_index = 0;
	}
	else
	{
		cap_val += Zero_Cross_Time;
		cap_index++;
		
		if (cap_index == CAP_STEP_NUM)
		{
			*pcounter_reg = cap_val;
			cap_val = 0;
			cap_index = 0;
		}
	}
}

#ifdef SENSORLESS	
	@near @interrupt @svlreg void ADC2_IRQHandler (void)
	{
		if (ADC_State == ADC_SYNC)
		{
			// Syncronous sampling
			
			u16 data;
			u8 delay;
			u16 bemf_threshold;

			// Reset bit
			bComHanderEnable = 0;
				
			//clear interrupt flag
			ADC2->CSR &= (u8)(~BIT7);
					
			//left align - read DRH first
			data = ADC2->DRH;
			data <<= 2;
			data |= (ADC2->DRL & 0x03);   
			
			switch( ADC_Sync_State )
			{
				case ADC_BEMF_INIT:
					ADC2->CSR = (u8)((Current_BEMF_Channel|BIT5)); 
					BEMF_Sample_Debounce = 0;
					Zero_Sample_Count = 0;
					ADC_Sync_State = ADC_BEMF_SAMPLE;
					SetSamplingPoint_BEMF();
				break;

				case ADC_BEMF_SAMPLE: 
					//detect zero crossing 
					if( Current_BEMF == BEMF_FALLING )
					{
						if( Z_Detection_Type == Z_DETECT_PWM_OFF )
						{
							bemf_threshold = BEMF_FALLING_THRESHOLD;
						}
						else
						{
							bemf_threshold = hNeutralPoint;
						}

						if (Ramp_Step > FORCED_STATUP_STEPS)
						{
							if( data <  bemf_threshold  )
							{
								Zero_Sample_Count++;
								BEMF_Sample_Debounce++;
								if( BEMF_Sample_Debounce >= BEMF_SAMPLE_COUNT )
								{
									hTim3Th -= hTim3Cnt;
									GetStepTime();

									#ifdef DEBUG_PINS
										Z_DEBUG_PORT ^= Z_DEBUG_PIN;
									#endif
	 
									SpeedMeasurement();

									bComHanderEnable = 1;

									BEMF_Sample_Debounce = 0;
								}
							}
							else
							{
								BEMF_Sample_Debounce = 0;
							}
						}
					}
					else
					{
						if( Z_Detection_Type == Z_DETECT_PWM_OFF )
						{
							bemf_threshold = BEMF_RISING_THRESHOLD;
						}
						else
						{
							bemf_threshold = hNeutralPoint;
						}
		
						if (Ramp_Step > FORCED_STATUP_STEPS)
						{
							if( data > bemf_threshold )
							{
								Zero_Sample_Count++;
								BEMF_Sample_Debounce++;
								if( BEMF_Sample_Debounce >= BEMF_SAMPLE_COUNT )
								{
									hTim3Th -= hTim3Cnt;
									GetStepTime();
								
									#ifdef DEBUG_PINS
										Z_DEBUG_PORT ^= Z_DEBUG_PIN;
									#endif

									SpeedMeasurement();

									bComHanderEnable = 1;

									BEMF_Sample_Debounce = 0;
								}
							}
							else
							{
								BEMF_Sample_Debounce = 0;
							}
						}
					}
				break;

				case ADC_CURRENT_INIT:
					ADC2->CSR = (ADC_CURRENT_CHANNEL|BIT5); 
					ADC_Sync_State = ADC_CURRENT_SAMPLE;
					SetSamplingPoint_Current();
				break;

				default:
				case ADC_USER_SYNC_INIT:
					ADC2->CSR = (ADC_USER_SYNC_CHANNEL|BIT5); 
					ADC_Sync_State = ADC_USER_SYNC_SAMPLE;
					SetSamplingPoint_User_Sync();
				break;

		
				case ADC_CURRENT_SAMPLE: 
					ADC_Buffer[ ADC_CURRENT_INDEX ] = data;
					break;

				case ADC_USER_SYNC_SAMPLE: 
					ADC_Buffer[ ADC_USER_SYNC_INDEX] = data;
					break;
			}

			// Store the current channel selected
			bCSR_Tmp = ADC2->CSR;

			// Set the Async sampling channel
			switch (ADC_Async_State)
			{
				default:
				case ADC_BUS_INIT:
					ADC2->CSR = (ADC_BUS_CHANNEL|BIT5); 
					ADC_Async_State = ADC_BUS_SAMPLE;
				break;
				
				case ADC_TEMP_INIT:
					ADC2->CSR = (ADC_TEMP_CHANNEL|BIT5); 
					ADC_Async_State = ADC_TEMP_SAMPLE;
				break;

				case ADC_NEUTRAL_POINT_INIT:
					ADC2->CSR = (ADC_NEUTRAL_POINT_CHANNEL|BIT5); 
					ADC_Async_State = ADC_NEUTRAL_POINT_SAMPLE;
				break;
				
				case ADC_USER_ASYNC_INIT:
					ADC2->CSR = (ADC_USER_ASYNC_CHANNEL|BIT5); 
					ADC_Async_State = ADC_USER_ASYNC_SAMPLE;
				break;
			}

			// Start asyncronous sampling
			#ifdef DEV_CUT_1
				// Disable ext. trigger
				ADC2->CR2 &= (u8)(~BIT6);
				//turn on ADC fix bug on cut1 device
				ADC2->CR1 |= BIT0;
				//Start ADC sample
				ADC2->CR1 |= BIT0;
			#else
				// Disable ext. trigger
				ADC2->CR2 &= (u8)(~BIT6);
				//Start ADC sample
				ADC2->CR1 |= BIT0;
			#endif

			ADC_State = ADC_ASYNC;
			
			if (bComHanderEnable == 1)
			{
				ComHandler();
			}
		}
		else
		{
			// Syncronous sampling
			u16 data;
			
			data = ADC2->DRH;
			data <<= 2;
			data |= (ADC2->DRL & 0x03);

			//clear interrupt flag
			ADC2->CSR &= (u8)(~BIT7);

			// Restore the sync ADC channel
			ADC2->CSR = bCSR_Tmp;
					
			
			// Configure syncronous sampling
			#ifdef DEV_CUT_1
				// Enable ext. trigger
				ADC2->CR2 |= BIT6;
				//turn on ADC fix bug on cut1 device
				ADC2->CR1 |= BIT0;  
			#else
				// Enable ext. trigger
				ADC2->CR2 |= BIT6;
			#endif

			// Manage async sampling
			switch (ADC_Async_State)
			{
				default:
				case ADC_BUS_SAMPLE:
					ADC_Buffer[ ADC_BUS_INDEX ] = data;
					ADC_Async_State = ADC_TEMP_INIT;
				break;

				case ADC_TEMP_SAMPLE:
					ADC_Buffer[ ADC_TEMP_INDEX ] = data;
					ADC_Async_State = ADC_NEUTRAL_POINT_INIT;
				break;

				case ADC_NEUTRAL_POINT_SAMPLE:
					ADC_Buffer[ ADC_NEUTRAL_POINT_INDEX ] = data;
					ADC_Async_State = ADC_USER_ASYNC_INIT;
				break;

				case ADC_USER_ASYNC_SAMPLE:
					ADC_Buffer[ ADC_USER_ASYNC_INDEX ] = data;
					ADC_Async_State = ADC_BUS_INIT;
				break;
			}
			
			ADC_State = ADC_SYNC;
			
		}
	}
#endif

#ifdef HALL
	@near @interrupt @svlreg void ADC2_IRQHandler (void)
	{
		u16 data;
		
		data = ADC2->DRH;
		data <<= 2;
		data |= (ADC2->DRL & 0x03);

		//clear interrupt flag
		ADC2->CSR &= (u8)(~BIT7);

		// Manage async sampling
		switch (ADC_Async_State)
		{
			default:			
			
			case ADC_CURRENT_SAMPLE: 
					ADC_Buffer[ ADC_CURRENT_INDEX ] = data;
					ADC_Async_State = ADC_BUS_INIT;
			break;			
			
			
			case ADC_BUS_SAMPLE:
				ADC_Buffer[ ADC_BUS_INDEX ] = data;
				ADC_Async_State = ADC_TEMP_INIT;
			break;

			case ADC_TEMP_SAMPLE:
				ADC_Buffer[ ADC_TEMP_INDEX ] = data;
				ADC_Async_State = ADC_NEUTRAL_POINT_INIT;
			break;

			case ADC_NEUTRAL_POINT_SAMPLE:
				ADC_Buffer[ ADC_NEUTRAL_POINT_INDEX ] = data;
				ADC_Async_State = ADC_USER_ASYNC_INIT;
			break;

			case ADC_USER_ASYNC_SAMPLE:
				ADC_Buffer[ ADC_USER_ASYNC_INDEX ] = data;
				ADC_Async_State = ADC_CURRENT_INIT;   //ADC_BUS_INIT;
			break;
		}

		// Set the Async sampling channel
		switch (ADC_Async_State)
		{
			default:
			
			
			case ADC_CURRENT_INIT:
				ADC2->CSR = (ADC_CURRENT_CHANNEL|BIT5); 
				ADC_Async_State = ADC_CURRENT_SAMPLE;
					SetSamplingPoint_Current();
			break;
			
			
			case ADC_BUS_INIT:
				ADC2->CSR = (ADC_BUS_CHANNEL|BIT5); 
				ADC_Async_State = ADC_BUS_SAMPLE;
			break;
			
			case ADC_TEMP_INIT:
				ADC2->CSR = (ADC_TEMP_CHANNEL|BIT5); 
				ADC_Async_State = ADC_TEMP_SAMPLE;
			break;

			case ADC_NEUTRAL_POINT_INIT:
				ADC2->CSR = (ADC_NEUTRAL_POINT_CHANNEL|BIT5); 
				ADC_Async_State = ADC_NEUTRAL_POINT_SAMPLE;
			break;
			
			case ADC_USER_ASYNC_INIT:
				ADC2->CSR = (ADC_USER_ASYNC_CHANNEL|BIT5); 
				ADC_Async_State = ADC_USER_ASYNC_SAMPLE;
			break;
		}

		// Configure syncronous sampling
		#ifdef DEV_CUT_1
			// Enable ext. trigger
			ADC2->CR2 |= BIT6;
			//turn on ADC fix bug on cut1 device
			ADC2->CR1 |= BIT0;  
		#else
			// Enable ext. trigger
			ADC2->CR2 |= BIT6;
		#endif		
	}
#endif

void DebugPinsOff(void)
{
#ifdef DEBUG_PINS
	Z_DEBUG_PORT &= (u8)(~Z_DEBUG_PIN);
	C_D_DEBUG_PORT &= (u8)(~C_D_DEBUG_PIN);
	AUTO_SWITCH_PORT &= (u8)(~AUTO_SWITCH_PIN);
	PWM_ON_SW_PORT &= (u8)(~PWM_ON_SW_PIN);
#endif
}

//Timer1 used for main motor PWM outputs and for triggering the ADC sample
//channels 1,2, and 3 are used.  Channel 4 is available
void Init_TIM1(void)
{
	//counter disabled, ARR preload register disabled, up counting, edge aligned mode
	TIM1->CR1 = BIT2;

	//Master Mode=TRGO-OC4REF,COMS only on COMG set, Preload CCxE, CCxNE, and OCxM bits
	TIM1->CR2 = (BIT6|BIT5|BIT4|BIT0);

	//slave mode disabled
	TIM1->SMCR = 0;

	#ifdef ETR_INPUT
		//Set external trigger filter
		TIM1->ETR = CURRENT_FILTER;
	#endif

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

	TIM1->CCER1 = 0;
	TIM1->CCER2 = 0;

	//select PWM mode 1, OC1 preload enabled
	TIM1->CCMR1 = CCMR_PWM;
	//select PWM mode 1, OC2 preload enabled
	TIM1->CCMR2 = CCMR_PWM;
	//select PWM mode 1, OC3 preload enabled
	TIM1->CCMR3 = CCMR_PWM;

	//select PWM mode 2, 
	TIM1->CCMR4 = BIT6|BIT5|BIT4|BIT3;

	//prescale = div1 @ 16MHz -> 62.5ns/count, Full scale = 4.09ms 
	TIM1->PSCRH = 0; 
	TIM1->PSCRL = 0;

	ToCMPxH( TIM1->ARRH, hArrPwmVal );
	ToCMPxL( TIM1->ARRL, hArrPwmVal );

	//disable repetition counter
	TIM1->RCR = 0;

	//default to 0% duty cycle
	ToCMPxH( TIM1->CCR1H, 0 );
	ToCMPxL( TIM1->CCR1L, 0 );
	//default to 0% duty cycle
	ToCMPxH( TIM1->CCR2H, 0 );
	ToCMPxL( TIM1->CCR2L, 0 );
	//default to 0% duty cycle
	ToCMPxH( TIM1->CCR3H, 0 );
	ToCMPxL( TIM1->CCR3L, 0 );

	#ifdef SENSORLESS
		ToCMPxH( TIM1->CCR4H, (hArrPwmVal-SAMPLING_POINT_DURING_TOFF_CNT) );
		ToCMPxL( TIM1->CCR4L, (hArrPwmVal-SAMPLING_POINT_DURING_TOFF_CNT) );
	#endif
	#ifdef HALL
		ToCMPxH( TIM1->CCR4H, hArrPwmVal - MILLIAMP_TOCNT (STARTUP_CURRENT_LIMITATION) );
		ToCMPxL( TIM1->CCR4L, hArrPwmVal - MILLIAMP_TOCNT (STARTUP_CURRENT_LIMITATION) );
	#endif

	//set dead time to 6us (with 62.5ns/count)
	TIM1->DTR = (u8)(hCntDeadDtime); 

	// Setup IDLE State 
	TIM1->OISR  = 0; // Default inactive 
	
	/* Set the Output Idle state & the Output N Idle state configuration */
	#if (PWM_U_HIGH_SIDE_IDLE_STATE == ACTIVE)
		TIM1->OISR |= (u8)(TIM1_OISR_OIS1);
	#endif
	#if (PWM_U_LOW_SIDE_IDLE_STATE == ACTIVE)
		TIM1->OISR |= (u8)(TIM1_OISR_OIS1N);
	#endif
	#if (PWM_V_HIGH_SIDE_IDLE_STATE ==  ACTIVE)
		TIM1->OISR |= (u8)(TIM1_OISR_OIS2);
	#endif
	#if (PWM_V_LOW_SIDE_IDLE_STATE == ACTIVE)
		TIM1->OISR |= (u8)(TIM1_OISR_OIS2N);
	#endif
	#if (PWM_W_HIGH_SIDE_IDLE_STATE ==  ACTIVE)
		TIM1->OISR |= (u8)(TIM1_OISR_OIS3);
	#endif
	#if (PWM_W_LOW_SIDE_IDLE_STATE == ACTIVE)
		TIM1->OISR |= (u8)(TIM1_OISR_OIS3N);