isrs.c

dsPIC30F在无传感器BLDC控制中的应用AN901(中文) 源代码1

		{
			if ((sector % 2) == 0)	control_flags2.FALLING_EDGE=FALSE;
			else							control_flags2.FALLING_EDGE=TRUE;
		}
		return;
	}
	
	// Blanking counter used to ignore samples just after
	// commutation so that diode action doesn't cause false
	// zero_crossing.

	if (blanking_counter > 0)
	{
		blanking_counter--;
		return;
	}
	// This locks out the possibility of a second zero crossing 
	// being detected in a particular sector.
	if (control_flags.ZERO_CROSS==TRUE)
		return;

	// Half_vdc is a simple filtered version of vdc/2
	half_vdc = ((half_vdc + (vdc>>1)) >> 1);

	// To add robustness to the zero crossing detection a programmable
	// number of samples of vph must be seen above (falling edge) 
	// or below (rising edge) before the actual crossing detection is enabled
	// The level_counter variable along with the LEVEL flag implement this

	if (level_counter==(unsigned char)user_parameters_RAM[33])
	{
		control_flags.LEVEL=TRUE;
	}

	if (control_flags2.FALLING_EDGE)
	{ 
		if ((level_counter < (unsigned char)user_parameters_RAM[33]) && (vph > half_vdc))
		{
			level_counter++;
		}
		if ((level_counter > 0) && (vph <= half_vdc))
		{
			level_counter--;
		}
	}
	else
	{
		if ((level_counter < (unsigned char)user_parameters_RAM[33]) && (vph < half_vdc))
		{
			level_counter++;
		}
		if ((level_counter > 0) && (vph >= half_vdc))
		{
			level_counter--;
		}
	}

	// Note that saved timestamp and timedelta data is stored in arrays with three
	// elements corresponding to the three phases. The conventions used are as follows:
	// Index 0 = Red
	// Index 1 = Yellow
	// Index 2 = Blue
	// Sector 0,3 = Blue Phase Crossing
	// Sector 1,4 = Yellow Phase Crossing
	// Sector 2,5 = Red Phase Crossing 
	if (control_flags.LEVEL==TRUE) // Only check for the crossings once LEVEL is true
	{
		// If we have seen sufficient valid samples above or below vdc/2 
		// and the current and previous samples are <= vdc/2 or >= vdc/2 (edge dependant)
		// then this is the valid crossing.
		if (((control_flags2.FALLING_EDGE) && (previous_sample <= half_vdc)\
				&& (vph <= half_vdc)) || \
			((control_flags2.FALLING_EDGE==FALSE) && (previous_sample >= half_vdc)\
				&& (vph >= half_vdc)))
		{
			// Grab latest value of QEI counter which is
			// being used in the 16 bit timer mode
			new_timestamp=POSCNT;

			// Calculate time between this zero crossing and the last one
			// The result of this calculation is correct irrespective
			// of counter rollover due to the elegance of 2's comp
			// arithmetic on unsigned variables.
			// Note that to ensure we use the correct previous zero crossing
			// timestamp the calculation is direction dependent
			if (control_flags.DIR == FORWARDS)
			{
				switch (sector)
				{
					case 0:
					case 3:	latest_delta=new_timestamp-previous_timestamps[0];
								break;
					case 1:
					case 4:	latest_delta=new_timestamp-previous_timestamps[2];
								break;
					case 2:
					case 5:	latest_delta=new_timestamp-previous_timestamps[1];
								break;
				}
			}
			else // going BACKWARDS
			{
				switch (sector)
				{
					case 0:
					case 3:	latest_delta=new_timestamp-previous_timestamps[1];
								break;
					case 1:
					case 4:	latest_delta=new_timestamp-previous_timestamps[0];
								break;
					case 2:
					case 5:	latest_delta=new_timestamp-previous_timestamps[2];
								break;
				}
			}

			// Calculate period measurement used for speed feedback
			// rather than just using 60 degree measurement use 180
			// degrees to get better resolution at high speeds
			// Only calculate period once per electrical cycle in sector 0
			// Note if you calculate period more often than once per electrical
			// cycle, you will get faster update rate but more ripple.
			if (sector==0)
			{
				period_measurement=new_timestamp-previous_timestamps[2];

				// Reset stall counter to denote a valid period measurement event has ocurred
				stall_counter=0;
			}

			// Calculate the number of counts of TIMER2 until next commutation
			// Note that TIMER2 and QEI are on same timebase
			// The divison by 2 is because the diffence between the zero
			// crossing of a phase and the previous one from another phase
			// is 60 electrical degrees. The natural offset between zero_crossing 
			// and the correct commutation is 30 electrical degress without any
			// phase advance. 
			commutation_time=((latest_delta)/2) - phase_advance;

			// Now check that phase advance didn't cause too small a value for
			// commutation time. This can occur because phase advance is only
			// calculated in the medium speed event routine based on the period
			// measurement for efficiency. The test is done at 1 because this is
			// the smallest value that can be loaded into PR2 for the TIMER2 interrupt
			// to function correctly as we reset TIMER2 to zero after writing to PR2
			if (commutation_time < 1) commutation_time=1;

			// Now check to see if commutation has failed. This is done
			// by comparing latest_delta with previous_delta and applying a
			// a tolerance check. 			
			check_value=(latest_delta*100)/previous_delta;
			
			if (((check_value > upper_tol) \
				|| (check_value < lower_tol))	\
				&& (check_counter==0))
			{
				// If sensorless failed then enter
				// acquistion mode2 again if auto re-acquire (user param 31) is enabled
				DISABLE_FIRING;
				IEC0bits.T2IE=FALSE;
				control_flags.SENSORLESS=FALSE;
				if (user_parameters_RAM[31])
				{
					control_flags.ACQUIRE2=TRUE;
					control_flags.ADCCONFIG=TRUE;
					control_flags2.RETRY_FLAG=FALSE;
				}
				else
				{
					run_state=FAULT;
					trip_state=LOST;
				}
				return;
			}
			else		// Valid zero crossing detected
			{
				// Update sector
				if (control_flags.DIR==FORWARDS)
				{
					if (sector==5) sector=0;
					else 				sector++;
				}
				else
				{
					if (sector==0) sector=5;
					else 				sector--;
				}
				
				// Commutate as sensorless working provided not acquiring
				// using method1
				if (control_flags.ACQUIRE1==FALSE)
				{
					// Load up period register
					PR2 = (unsigned int) commutation_time;
					TMR2=0;
					// Now clear off interrupt flag and enable interrupt
					IFS0bits.T2IF = 0;
					IEC0bits.T2IE = 1;
					
					// Load up guard timer implemented in Timer 1 with
					// double the latest time delta between zero crossings
					PR1=latest_delta<<1;
					TMR1=0;
					
					// Clear interrupt flag just in case writing to timer
					// generated an interrupt.
					IFS0bits.T1IF = 0;
					// Now enable interrupt.
					IEC0bits.T1IE = 1;
				}
				else
				{
					// Incrementing acquire counter indicates a valid zero x
					// event has just occurred.
					// acquire counter is reset to zero by code in T2ISR if
					// a zero X event is missed during acquistion
					acquire_counter++;

					if (acquire_counter > 1) 
					{	// i.e. if have had two consequtive zero X events
						// Disable T2 interrupts ready for transition to 
						// closed loop commutation
						IEC0bits.T2IE = 0;

						// Set flags to indicate now running sensorless
						control_flags.ACQUIRE1=FALSE;
						control_flags.SENSORLESS=TRUE;
						run_state=RUNNING;
						// load up check counter to disable zeor X tolerance
						// checks for 1 elec cycle
						check_counter=6;
						// set up T2 to generate interrupt at correct time
						PR2 = (unsigned int) commutation_time;
						TMR2=0;
						// Now clear off interrupt flag and enable interrupt
						IFS0bits.T2IF = 0;
						IEC0bits.T2IE = 1;

					}
				}
			}
			// End of else associated with a valid zero crossing
			// Note that if you fail tolerance check code returns before
			// this point.

			// Save off timestamp info ready for next zero crossing

			if (control_flags.DIR==FORWARDS)
			{
				switch (sector)
				{
					case 0:	
					case 3:	previous_timestamps[0] = new_timestamp;
								break;
					case 1:	
					case 4:	previous_timestamps[2] = new_timestamp;
								break;
					case 2:	
					case 5:	previous_timestamps[1] = new_timestamp;
								break;
				}
			}
			else
			{
				switch (sector)
				{
					case 0:	
					case 3:	previous_timestamps[1] = new_timestamp;
								break;
					case 1:	
					case 4:	previous_timestamps[0] = new_timestamp;
								break;
					case 2:	
					case 5:	previous_timestamps[2] = new_timestamp;
								break;
				}	
			}
			// Save the latest 60 degree time difference for next
			// zero crossing tolerance check
			previous_delta=latest_delta;
			// Set flag to indicate a valid zero Xing occurred
			control_flags.ZERO_CROSS=TRUE;
			level_counter=0;
			control_flags.LEVEL=FALSE;

		}	// End of the if statement detecting zero X
	}	// End of the if (control_flags.LEVEL=TRUE)
	// Save off current phase voltage sample ready for next call
	
	previous_sample=vph;

	return;
}

// This function implements current control using the bus current
// It is intended to be called after the latest ADC reading of ibus
// is availble.
// As the control effort calculated is directly loaded into the
// the PWM duty cycle register, 0 effort is equal to 50% duty cycle
// 50% duty cycle gives 0 average phase voltage applied if current
// is continuous.
static void current_control(void)
{

	static long current_integral;
	static int previous_current_error;
	int current_error;
	long proportional_term;
	long derivative_term;
	int control_output;
	long temp;

	// Check to see whether current control loop should be active
	// Whenever switches are turned off or using voltage control
	// the current control should be inactive
	// Strictly speaking should check for faults and initializing
	// and exit, but will always enter the standby before can run
	// or start so there is no need
	// If current control not active clear off integral term and
	// also disable D term for first pass by setting flag

	if ((run_state == STANDBY) || (control_flags.ACQUIRE2==TRUE) \
		|| (control_flags2.ROTATION_CHECK==TRUE))
	{
		current_integral=0;
		control_flags2.D_TERM_DISABLE=TRUE;
		return;
	}

	if ((run_state == STARTING) && (user_parameters_RAM[40]==TRUE))
	{
		current_integral=0;
		control_flags2.D_TERM_DISABLE=TRUE;
		return;
	}

	if ((run_state == RUNNING) && ((user_parameters_RAM[1]==OPEN_VOLTS) \
		|| (user_parameters_RAM[1]==CLOSED_VOLTS)))
	{
		current_integral=0;
		control_flags2.D_TERM_DISABLE=TRUE;
		return;
	}

	// Form the current error by subtracting the ibus from the current_demand
	// Also account for the fact that zero ibus is biased by 
	// approx 2.5V so that can measure bipolar currents
	// ibus_offset is the ADC value of the ibus signal for zero current
	current_error=(current_demand+ibus_offset)-ibus;

	// Start off by assuming not in voltage limit
	control_flags.VOLTS_LIMIT=FALSE;

	// All calculated quantities will be shifted back down at the 
	// end by 512. PID gains are effectively scaled up by 512.
	// This is done to allow fractional gains without
	// severe rounding issues which causes quantization noise etc.
	
	proportional_term=((long)current_error*(long)iloop_p_gain);

	derivative_term=((long)(current_error-previous_current_error)\
							*(long)iloop_d_gain);

	if (control_flags2.D_TERM_DISABLE)
		derivative_term=0;

	// Now calculate P+D terms and put them in temp
	temp=proportional_term+derivative_term;

	if (temp > pos_duty_limit)
	{
		control_flags.VOLTS_LIMIT=TRUE;
		control_output=(pos_duty_limit/512)+ZERO_DUTY;
		// If in limit due to just P term then clear
		// off I term so that when come out of limit
		// I term can smoothly take over to eliminate 
		// steady state error.
		current_integral=0;
	}
	else
	{
		if (temp < neg_duty_limit)
		{
			control_flags.VOLTS_LIMIT=TRUE;
			control_output=(neg_duty_limit/512)+ZERO_DUTY;
			// If in limit due to just P term then clear
			// off I term so that when come out of limit
			// I term can smoothly take over to eliminate 
			// steady state error.
			current_integral=0;
		}
	}
	// Now do integral term if not in torque limit already due to P+D term
	if (control_flags.VOLTS_LIMIT==FALSE)
	{
		// Update Integral term here but this will be overwritten if
		// found to be in limit later on in code
		current_integral+=((long)current_error * (long)iloop_i_gain);
		// Add I term to P+D terms
		temp+=current_integral;

		// If sum of PID terms pushes you into positive limit
		if (temp > pos_duty_limit)
		{
			control_flags.VOLTS_LIMIT=TRUE;
			control_output=(pos_duty_limit/512)+ZERO_DUTY;
			// Now calculate integrator limit and clamp
			current_integral = pos_duty_limit - proportional_term - derivative_term;
		}
		else
		{
			// If sum of P+I terms pushes you into negative limit
			if (temp < neg_duty_limit)
			{
				control_flags.VOLTS_LIMIT=TRUE;
				control_output=(neg_duty_limit/512)+ZERO_DUTY;
				// Now calculate integrator limit and clamp
				current_integral=neg_duty_limit - proportional_term - derivative_term;
			}
			else
			// Calculate control output based on unclamped PID terms
			{
				// Temp still contains unclamped P+I term so no need to recalculate
				control_output=(temp/512)+ZERO_DUTY;
			}
		}