mc_stm8s_bldc_drive.c

STM8S105 BLDC源代码

u8 cap_index = 0;
u8 first_cap = 0;

u8 BEMF_Falling_Factor;
u8 BEMF_Rising_Factor;

#define BEMF_RISE_DELAY_FACTOR 128 // Strtup values
#define BEMF_FALL_DELAY_FACTOR 128

// BEMF voltage treshold
#define BEMF_RISING_THRESHOLD  (u16)((BEMF_RISING_THRESHOLD_V * 1024)/(EXPECTED_MCU_VOLTAGE))
#define BEMF_FALLING_THRESHOLD (u16)((BEMF_FALLING_THRESHOLD_V * 1024)/(EXPECTED_MCU_VOLTAGE))

u16 Zero_Sample_Count;
u16 Motor_Frequency;

u8 Motor_Stall_Count;
#define MOTOR_STALL_THRESHOLD 6

#ifdef HALL
	u8 bHallStartStep;

	const u8 bHallSteps_CW[8] = {
		HALL_SENSOR_STEPS_CW_000,
		HALL_SENSOR_STEPS_CW_001,
		HALL_SENSOR_STEPS_CW_010,
		HALL_SENSOR_STEPS_CW_011,
		HALL_SENSOR_STEPS_CW_100,
		HALL_SENSOR_STEPS_CW_101,
		HALL_SENSOR_STEPS_CW_110,
		HALL_SENSOR_STEPS_CW_111
	};

	const u8 bHallSteps_CCW[8] = {
		HALL_SENSOR_STEPS_CCW_000,
		HALL_SENSOR_STEPS_CCW_001,
		HALL_SENSOR_STEPS_CCW_010,
		HALL_SENSOR_STEPS_CCW_011,
		HALL_SENSOR_STEPS_CCW_100,
		HALL_SENSOR_STEPS_CCW_101,
		HALL_SENSOR_STEPS_CCW_110,
		HALL_SENSOR_STEPS_CCW_111
	};

	u8* bHallSteps = bHallSteps_CW;
#endif

#define MAX_BUS_VOLTAGE16 (u16)((MAX_BUS_VOLTAGE*(u32)1024)/BUSV_CONVERSION)
#define MIN_BUS_VOLTAGE16 (u16)((MIN_BUS_VOLTAGE*(u32)1024)/BUSV_CONVERSION)
#define BRAKE_HYSTERESIS  (u16)((MAX_BUS_VOLTAGE16/16)*15)

u8 bComHanderEnable = 0;
u16 hTim3Cnt = 0,hTim3Th = 0;

static pu16 pcounter_reg;
static pu16 pDutyCycleCounts_reg;

u16 tmp_u16;
u8 tmp_u8;
u8 tmp_TIM1_CCMR1;
u8 tmp_TIM1_CCMR2;
u8 tmp_TIM1_CCMR3;
u8 tmp_TIM1_CCER1;
u8 tmp_TIM1_CCER2;

void dev_driveInit(pvdev_device_t pdevice)
{
	PBLDC_Struct_t pBLDC_Struct = Get_BLDC_Struct();
	g_pBLDC_Struct = pBLDC_Struct;
	g_pMotorVar = pBLDC_Struct->pBLDC_Var;
	g_pDevice = pdevice;

#if (BEMF_SAMPLING_METHOD == BEMF_SAMPLING_MIXED)
	hDutyCntTh = (u16)(((u32)hArrPwmVal*DUTY_CYCLE_TH_TON)/100);
#endif

	hCntDeadDtime = (u16)(((u32)g_pBLDC_Struct->pBLDC_Const->hDeadTime * STM8_FREQ_MHZ )/1000);

	// Current Limitation
	CurrentLimitCnt = (u16)(MILLIAMP_TOCNT (g_pBLDC_Struct->pBLDC_Const->hCurrent_Limitation));

#ifdef SENSORLESS
	pcounter_reg = &(pdevice->regs.r16[VDEV_REG16_BEMF_COUNTS]);
#endif
#ifdef HALL
	pcounter_reg = &(pdevice->regs.r16[VDEV_REG16_HALL_COUNTS]);
#endif
	
	pDutyCycleCounts_reg = &(pdevice->regs.r16[VDEV_REG16_BLDC_DUTY_CYCLE_COUNTS]);

	Init_TIM1();
	Init_TIM2();
	Init_ADC();  
	#ifdef DEBUG_PINS
		DebugPinsOff();
	#endif
	
	#ifdef LS_GPIO_CONTROL

		//Init LS_GPIO_PORT
		#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
		LS_A_PORT->DDR |= LS_A_PIN;
		LS_A_PORT->CR1 |= LS_A_PIN;
		LS_A_PORT->CR2 |= LS_A_PIN;
		#if (PWM_V_LOW_SIDE_POLARITY == ACTIVE_HIGH)
			LS_B_PORT->ODR &= (u8)(~LS_B_PIN);
		#else
			LS_B_PORT->ODR |= LS_B_PIN;
		#endif
		LS_B_PORT->DDR |= LS_B_PIN;
		LS_B_PORT->CR1 |= LS_B_PIN;
		LS_B_PORT->CR2 |= LS_B_PIN;
		#if (PWM_W_LOW_SIDE_POLARITY == ACTIVE_HIGH)
			LS_C_PORT->ODR &= (u8)(~LS_C_PIN);
		#else
			LS_C_PORT->ODR |= LS_C_PIN;
		#endif
		LS_C_PORT->DDR |= LS_C_PIN;
		LS_C_PORT->CR1 |= LS_C_PIN;
		LS_C_PORT->CR2 |= LS_C_PIN;

	#endif

	vtimer_SetTimer(ADC_SAMPLE_TIMER,ADC_SAMPLE_TIMEOUT,&Application_ADC_Manager);
}

void dev_driveIdle(void)
{
}

MC_FuncRetVal_t dev_driveStartUpInit(void)
{
	// Set the correct Step table
	if (g_pBLDC_Struct->pBLDC_Var->hTarget_rotor_speed > 0)
	{
		BEMFSteps = BEMFSteps_CW;
		PhaseSteps = PhaseSteps_CW;
		Fast_Demag_Steps = Fast_Demag_Steps_CW;
		
		#ifdef LS_GPIO_CONTROL
			LS_Steps = LS_Steps_CW;
			LS_Steps_SW = LS_Steps_SW_CW;
		#endif
		
		#ifdef HALL
			bHallSteps = bHallSteps_CW;
		#endif
	}
	else
	{
		BEMFSteps = BEMFSteps_CCW;
		PhaseSteps = PhaseSteps_CCW;
		Fast_Demag_Steps = Fast_Demag_Steps_CCW;
		
		#ifdef LS_GPIO_CONTROL
			LS_Steps = LS_Steps_CCW;
			LS_Steps_SW = LS_Steps_SW_CCW;
		#endif
		
		#ifdef HALL
			bHallSteps = bHallSteps_CCW;
		#endif
	}

	StartUpStatus = STARTUP_BOOTSTRAP;
	vtimer_SetTimer(MTC_ALIGN_RAMP_TIMER,5,0);
 
	return FUNCTION_ENDED;
}

MC_FuncRetVal_t dev_driveStartUp(void)
{
	switch (StartUpStatus)
	{
	case STARTUP_IDLE:
		break;

	case STARTUP_BOOTSTRAP:
		if (BootStrap() == TRUE)
		{
			Align_State = ALIGN_IDLE;
			#ifdef DEBUG_PINS
				AUTO_SWITCH_PORT &= (u8)(~AUTO_SWITCH_PIN);
			#endif
			#ifdef SENSORLESS
				StartUpStatus = STARTUP_ALIGN;
			#endif
			#ifdef HALL
				TIM2_InitCapturePolarity();

				//preload next step
				Current_Step = bHallStartStep;
				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;

				Previous_Zero_Cross_Time = 0;
				Z_Detection_Type = Z_DETECT_PWM_ON;

				StartUpStatus = STARTUP_START;
			#endif
			first_cap = 0;
		}
		break;

	case STARTUP_ALIGN:
		if( AlignRotor() )
		{
			StartUpStatus = STARTUP_START;
		}
		break;

	case STARTUP_START:
		#ifdef SENSORLESS
			MTC_Status |= MTC_STEP_MODE;
		#endif
		#ifdef HALL
			MTC_Status |= MTC_STEP_MODE;
		#endif
		MTC_Status &= (u8)(~(MTC_STARTUP_FAILED|MTC_OVER_CURRENT_FAIL|MTC_LAST_FORCED_STEP|MTC_MOTOR_STALLED));
		
		// Reset software threshold
		hTim3Th = 0;

		StartMotor();

		// Reset software counter
		hTim3Cnt = 0;

		// Enable update interrupt
		TIM1->IER |= BIT0;

		#ifdef SENSORLESS
			StartUpStatus = STARTUP_RAMPING;
		#endif
		#ifdef HALL
			vtimer_SetTimer(DEV_DUTY_UPDATE_TIMER,SPEED_PID_SAMPLING_TIME,&dev_BLDC_driveUpdate);
			StartUpStatus = STARTUP_IDLE;
			return FUNCTION_ENDED;
		#endif
		break;

	case STARTUP_RAMPING:
		if( (MTC_Status & MTC_STEP_MODE) == 0 )
		{
			vtimer_SetTimer(DEV_DUTY_UPDATE_TIMER,SPEED_PID_SAMPLING_TIME,&dev_BLDC_driveUpdate);
			StartUpStatus = STARTUP_IDLE;
			return FUNCTION_ENDED;
		}
		else
		{
			if( MTC_Status & (MTC_STARTUP_FAILED|MTC_OVER_CURRENT_FAIL) )
			{
				StartUpStatus = STARTUP_IDLE;
				dev_driveStop();
				return FUNCTION_ERROR;
			}
		}
		break;
	}
	return FUNCTION_RUNNING;
}

MC_FuncRetVal_t dev_driveRun(void)
{ 
	if( ((MTC_Status & MTC_OVER_CURRENT_FAIL) == MTC_OVER_CURRENT_FAIL) ||
		((MTC_Status & MTC_MOTOR_STALLED) == MTC_MOTOR_STALLED) )
	{
		dev_driveStop();
		return FUNCTION_ERROR;
	}
	return FUNCTION_RUNNING;
}

MC_FuncRetVal_t dev_driveStop(void)
{
	vtimer_KillTimer(DEV_DUTY_UPDATE_TIMER);
	StopMotor();
	#ifdef DEBUG_PINS
		DebugPinsOff();
	#endif
	*pcounter_reg = 0;

	// Check toggle mode
	if (g_pBLDC_Struct->pBLDC_Var->bToggleMode)
	{
		g_pBLDC_Struct->pBLDC_Var->hTarget_rotor_speed = -g_pBLDC_Struct->pBLDC_Var->hTarget_rotor_speed;
	}
	return FUNCTION_ENDED;
}

MC_FuncRetVal_t dev_driveWait(void)
{
	#ifdef ACTIVE_BRAKE
		static u8 st_tim = 0;
		if (st_tim == 0)
		{
			vtimer_SetTimer(MTC_ALIGN_RAMP_TIMER,BRAKE_DURATION,0);
			st_tim = 1;
			BrakeMotor();
		}
		if (vtimer_TimerElapsed(MTC_ALIGN_RAMP_TIMER))
		{
			st_tim = 0;
			StopMotor();
			return FUNCTION_ENDED;
		}
		else
		{
			return FUNCTION_RUNNING;
		}
	#else
		return FUNCTION_ENDED;
	#endif
}

void dev_BLDC_driveUpdate(void)
{
	vtimer_SetTimer(DEV_DUTY_UPDATE_TIMER,SPEED_PID_SAMPLING_TIME,&dev_BLDC_driveUpdate);
	#if (CURRENT_CONTROL_MODE == VOLTAGE_MODE)
		#if (SPEED_CONTROL_MODE == CLOSED_LOOP)
			g_pBLDC_Struct->pBLDC_Var->hDuty_cycle = Set_Duty(*pDutyCycleCounts_reg);
		#else
			Set_Duty(*pDutyCycleCounts_reg);
		#endif
	#endif
	#if (CURRENT_CONTROL_MODE == CURRENT_MODE)
		#if (SPEED_CONTROL_MODE == CLOSED_LOOP)
			g_pBLDC_Struct->pBLDC_Var->hCurrent_reference = Set_Current(*pDutyCycleCounts_reg);
		#else
			Set_Current(*pDutyCycleCounts_reg);
		#endif
	#endif
}

@near @interrupt @svlreg void TIM1_UPD_OVF_TRG_BRK_IRQHandler (void)
{
	#ifdef DEBUG_PINS
		static u16 bkin_blink_cnt = 0;
	#endif
	if( (TIM1->SR1 & BIT7) == BIT7 )
	{
		TIM1->SR1 &= (u8)(~BIT7);
		
		#ifdef LS_GPIO_CONTROL
			// Disable low side
			
			// 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
			// Deactivate B
			#if (PWM_V_LOW_SIDE_POLARITY == ACTIVE_HIGH)
				LS_B_PORT->ODR &= (u8)(~LS_B_PIN);
			#else
				LS_B_PORT->ODR |= LS_B_PIN;
			#endif
			// Deactivate C
			#if (PWM_W_LOW_SIDE_POLARITY == ACTIVE_HIGH)
				LS_C_PORT->ODR &= (u8)(~LS_C_PIN);
			#else
				LS_C_PORT->ODR |= LS_C_PIN;
			#endif
		#endif
		
		StartUpStatus = STARTUP_IDLE;

		g_pDevice->regs.r16[VDEV_REG16_HW_ERROR_OCCURRED] |= OVER_CURRENT;
		#ifdef DEBUG_PINS
			DebugPinsOff();
			
			// If the BKIN is still active turn on the led to signal this condition
			if (((TIM1->SR1 & BIT7) == BIT7 ) && (bkin_blink_cnt <= 32768))
			{
				Z_DEBUG_PORT |= Z_DEBUG_PIN;
				C_D_DEBUG_PORT |= C_D_DEBUG_PIN;
				AUTO_SWITCH_PORT |= AUTO_SWITCH_PIN;
				PWM_ON_SW_PORT |= PWM_ON_SW_PIN;
			}
			bkin_blink_cnt++;
		#endif
		
		
	} 
	else
	{
		// Update management only if BRK is not occurred
		if ((TIM1->SR1 & BIT0) == BIT0)
		{
			hTim3Cnt++;

			// Check for match
			if (hTim3Cnt >= hTim3Th)
			{
				ComHandler();
			}

			// Clear Flag
			TIM1->SR1 &= (u8)(~BIT0);
		}
	}
}

void Application_ADC_Manager( void )
{
	vtimer_SetTimer(ADC_SAMPLE_TIMER,ADC_SAMPLE_TIMEOUT,&Application_ADC_Manager);
	
	GetCurrent();
	GetSyncUserAdc();
	GetBusVoltage();
	GetTemperature();
	GetNeutralPoint();
	GetAsyncUserAdc();
}

void GetCurrent(void)
{
	u16 hVal;
	disableInterrupts();
	hVal = ADC_Buffer[ADC_CURRENT_INDEX];
	enableInterrupts();

	BLDC_Set_Current_measured((u16)(ADC_TOMILLIAMP(hVal)/100));
}

void GetSyncUserAdc(void)
{
	u16 hSyncUserAdc;
	
	disableInterrupts();
	hSyncUserAdc = ADC_Buffer[ADC_USER_SYNC_INDEX];
	enableInterrupts();
	
	// Call User defined function with hUserAdc parameter
}

#ifdef BUS_VOLTAGE_MEASUREMENT
	void GetBusVoltage( void )
	{
		u16 data;
		
		disableInterrupts();
		data = ADC_Buffer[ ADC_BUS_INDEX ];
		enableInterrupts();
		
		if (data > MAX_BUS_VOLTAGE16)
		{

			#ifdef DISSIPATIVE_BRAKE
				#if (DISSIPATIVE_BRAKE_POL == DISSIPATIVE_BRAKE_ACTIVE_HIGH)
					DISSIPATIVE_BRAKE_PORT->ODR |= DISSIPATIVE_BRAKE_BIT;
				#else
					DISSIPATIVE_BRAKE_PORT->ODR &= (u8)(~DISSIPATIVE_BRAKE_BIT);
				#endif
			#else			
				g_pDevice->regs.r16[VDEV_REG16_HW_ERROR_OCCURRED] |= BUS_OVERVOLTAGE;
				g_pDevice->regs.r16[VDEV_REG16_HW_ERROR_ACTUAL] |= BUS_OVERVOLTAGE;
			#endif
		}
		if (data < BRAKE_HYSTERESIS)
		{
			#ifdef DISSIPATIVE_BRAKE
				#if (DISSIPATIVE_BRAKE_POL == DISSIPATIVE_BRAKE_ACTIVE_HIGH)
					DISSIPATIVE_BRAKE_PORT->ODR &= (u8)(~DISSIPATIVE_BRAKE_BIT);
				#else
					DISSIPATIVE_BRAKE_PORT->ODR |= DISSIPATIVE_BRAKE_BIT;
				#endif
			#else
				g_pDevice->regs.r16[VDEV_REG16_HW_ERROR_ACTUAL] &= (u8)(~BUS_OVERVOLTAGE);
			#endif
		}
		if (data < MIN_BUS_VOLTAGE16)
		{
			g_pDevice->regs.r16[VDEV_REG16_HW_ERROR_OCCURRED] |= BUS_UNDERVOLTAGE;
			g_pDevice->regs.r16[VDEV_REG16_HW_ERROR_ACTUAL] |= BUS_UNDERVOLTAGE;      
		}
		else
		{
			g_pDevice->regs.r16[VDEV_REG16_HW_ERROR_ACTUAL] &= (u8)(~BUS_UNDERVOLTAGE);
		}    
	    
		g_pDevice->regs.r16[VDEV_REG16_BOARD_BUS_VOLTAGE] = data;

		BLDC_Set_Bus_Voltage((u16)((data * BUSV_CONVERSION)/1024));
	}
#endif

#ifndef BUS_VOLTAGE_MEASUREMENT
	void GetBusVoltage( void )
	{
		BLDC_Set_Bus_Voltage((u16)(BUS_VOLTAGE_VALUE));
	}
#endif

#ifdef HEAT_SINK_TEMPERATURE_MEASUREMENT
	void GetTemperature(void)
	{
		u16 data;
		u8 temp;

		disableInterrupts();
		data = ADC_Buffer[ ADC_TEMP_INDEX ];
		enableInterrupts();
		g_pDevice->regs.r8[VDEV_REG8_HEATSINK_TEMPERATURE] = (u8)(data >> 2);

		if (data > NTC_THRESHOLD)
		{
			g_pDevice->regs.r16[VDEV_REG16_HW_ERROR_OCCURRED] |= HEATSINK_TEMPERATURE;
			g_pDevice->regs.r16[VDEV_REG16_HW_ERROR_ACTUAL] |= HEATSINK_TEMPERATURE;      
		}
		if (data < NTC_HYSTERIS)
		{
			g_pDevice->regs.r16[VDEV_REG16_HW_ERROR_ACTUAL] &= (u8)(~HEATSINK_TEMPERATURE);
		}

		temp = (u8)(TEMP_SENS_T0 + (((s16)data - TEMP_SENS_BETA)/TEMP_SENS_ALPHA));
		BLDC_Set_Heatsink_Temperature((u8)(temp));
	}
#endif

#ifndef HEAT_SINK_TEMPERATURE_MEASUREMENT
	void GetTemperature(void)
	{
		BLDC_Set_Heatsink_Temperature((u8)(HEAT_SINK_TEMPERATURE_VALUE));
	}
#endif

void GetNeutralPoint(void)
{
	u16 data;
	disableInterrupts();
	data = ADC_Buffer[ ADC_NEUTRAL_POINT_INDEX ];
	enableInterrupts();

	hNeutralPoint = data;
}

void GetAsyncUserAdc(void)
{
	u16 hAsyncUserAdc;

	disableInterrupts();
	hAsyncUserAdc = ADC_Buffer[ADC_USER_ASYNC_INDEX];
	enableInterrupts();