isrs.c

关于无传感器BLDC使用DSPIC现实的源码

								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[40]==TRUE) \
		&& (control_flags2.WINDMILLING==FALSE)))
	{
		current_integral=0;
		control_flags2.D_TERM_DISABLE=TRUE;
		return;
	}

	if ((run_state == RUNNING) && ((user_parameters[1]==OPEN_VOLTS) \
		|| (user_parameters[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;
			}
		}
	}
	// Now do final check to ensure control_output is not < 0
	// as this would cause large positive value to be loaded
	// into duty cycle registers!
	if (control_output < 0) control_output=0;

	// Now load calculated value into duty cycle registers
	// No need to disable writes to duty regsiters here
	// as we are not going to allow this code to be interrupted
	// (except by a fault) and assuming next pwm cycle starts after
	// registers are written.
	// Compensate calculated output due to fact that firing is inverted
	control_output=FULL_DUTY-control_output;		
	PDC1=(unsigned int)control_output;
	PDC2=(unsigned int)control_output;
	PDC3=(unsigned int)control_output;
	// Save current_error for use by D term next pass
	previous_current_error=current_error;
	// Clear D_TERM_DISABLE as this is only used for the first pass after
	// loop becomes active as don't have a valid previous error to base
	// D term calculation on.
	control_flags2.D_TERM_DISABLE=FALSE;
	return;
}

// This function is designed to determine the position of the rotor
// while the motor is turning but there is no current flowing.
// It looks for the sequence and timing between the rising edges of
// all three phase voltages. The sequence determines direction.
// The timing is used to set up the sensorless energisation
// This function will be called during starting and if lost during
// running.
// A rising edge of the 3 vph occur at the start of the following sectors
// Red = sector 5
// Yellow = sector 1
// Blue = sector 3	

static void acquire_position(void)
{

	static unsigned int previous_vph_red;
	static unsigned int previous_vph_yellow;
	static unsigned int previous_vph_blue;

	static unsigned char level_count_red;
	static unsigned char level_count_yellow;
	static unsigned char level_count_blue;

	static unsigned int previous_edge_time;
	static unsigned char retry_counter;
	unsigned int new_edge_time;
	unsigned int one_twenty_deg;
	unsigned int thirty_deg;
	

	if (control_flags.ADCCONFIG==TRUE)
	{
		control_flags.ADCCONFIG=FALSE;
		control_flags.ACQUIRE2_RED=FALSE;
		control_flags.ACQUIRE2_YELLOW=FALSE;
		control_flags.ACQUIRE2_BLUE=FALSE;
		previous_vph_red=0;
		previous_vph_yellow=0;
		previous_vph_blue=0;
		level_count_red=0;
		level_count_yellow=0;
		level_count_blue=0;
		previous_edge_time=0;
		if (control_flags2.RETRY_FLAG==FALSE)
			retry_counter=NO_RETRIES;
		control_flags2.WINDMILLING=FALSE;
		
		#ifdef DEBUG
			LATDbits.LATD15=1;
			indx=0;
		#endif
		return;
	}
	// This capture of data can be useful for solving acquisition
	// problems by inspecting values using ICD or ICE4000
	#ifdef DEBUG
		//data_log[indx]=vph_red;
		//data_log2[indx]=vph_yellow;
		//data_log3[indx]=vph_blue;
		//data_log4[indx]=POSCNT;
		if (indx<255)
			indx++;
	#endif

	// Check for failure to acquire by monitoring retry counter
	// The retry counter is loaded first time acqusition starts
	// Every time same phase is detected as rising before another phase
	// the counter is decremented.
	// If the retry counter has reached zero and no other trip exists
	if ((retry_counter==0) && (trip_state==0))
	{
		run_state=FAULT;
		trip_state=FAILED_TO_START;
		control_flags.ACQUIRE2=FALSE;
		return;
	}
	// Note that all three sections of code below are similar in function but 
	// differ according to the particular sectors, directions etc
	// Red phase code is fully commented, Yellow and Blue are not

	/******************** START OF RED PHASE CODE **********************************/
	
	// To provide robust crossing detection, a similar scheme as used for zero
	// crossing is used except need a separate counter for each phase.
	// The level counter is only incremented if we see VPH below the acquire_threshold
	// and it has not reached the level_threshold. We are looking for a rising edge

	if ((vph_red < vph_red_threshold) \
		&& (level_count_red < (unsigned char)user_parameters[35]))
	{
			level_count_red++;
	}

	// If we have seen sufficient samples below the acquire_threshold
	if (level_count_red == (unsigned char)user_parameters[35])
	{
		// If previous sample and current sample are >= acquire_threshold
		// then we treat this as a valid rising edge
		if ((previous_vph_red >= vph_red_threshold) \
			&& (vph_red >= vph_red_threshold))
		{
			// Grab new timestamp from free-running counter (QEI in this case)
			new_edge_time=POSCNT;

			// If ACQUIRE2_RED is already set this means that have two edges
			// of RED before another phase which is not correct
			// If detected then reinitialize acquision and decrement retry_counter
			if (control_flags.ACQUIRE2_RED)
			{
				#ifdef DEBUG
					LATDbits.LATD15=0;
				#endif
				// The RETRY_FLAG is set so initialization code does not reload
				// the retry_counter
				control_flags2.RETRY_FLAG=TRUE;
				if (retry_counter)
					retry_counter--;
				// Setting this flag will force re-initialization next call
				control_flags.ADCCONFIG=TRUE;
				return;
			}
			// Reset level_counter so that VPH has to fall back beneath
			// acquire_threshold before rising check done again.
			level_count_red=0;

			// Now check to see if a previous phase's rising edge has already
			// detected. The sequence of the rising edges determines direction
			// and therefore also which sector has just been entered.
			if (control_flags.ACQUIRE2_BLUE==TRUE)
			{
				#ifdef DEBUG
					LATDbits.LATD15=0;
				#endif

				// Update the sector that rotor is in
				sector=5;
				// Update OVDCON with appropriate value
				OVDCON=SECTOR5_OVERRIDE;

				// Calculate times for 120 and 30 electrical degrees
				one_twenty_deg=new_edge_time-previous_edge_time;
				thirty_deg=one_twenty_deg/4;
				
				// Calculate period measurement so speed loop has valid data
				// and ensure don't try to re-acquire at too high a speed. 
				// The period measurement is 180 electrical degrees
				period_measurement=one_twenty_deg+(one_twenty_deg/2);
				// Reset stall counter to denote period has been calculated
				stall_counter=0;

				// Check to see if windmilling i.e. motor is turning in opposite
				// direction to demanded direction.
				// If it is, then will open loop energise motor with
				// current control and gradually reduce output frequency until
				// stationary. Will then start using normal routine.
				if (control_flags.DIR==BACKWARDS) // If windmilling
				{
					// Deliberately use braking energisation
					// This is done by using commutation that is 180 electrical
					// degrees offset from actual position.
					// Therefore sector 5 maps to sector 2
					sector=2;
					OVDCON=SECTOR2_OVERRIDE;
					// Set WINDMILLING flag so that starting code enters a different
					// mode of operation which gradually reduces commutation frequency
					// and then locks and ramps in the correct direction
					control_flags2.WINDMILLING=TRUE;
					// Clear ROTATION_CHECK flag as valid rotation is occuring
					control_flags2.ROTATION_CHECK=FALSE;
					// Clear ACQUIRE2 flag as system going to be energised open loop
					control_flags.ACQUIRE2=FALSE;
					// Initial commutation period = 60 degrees
					new_PR2=one_twenty_deg/2;