#include #include #include #include #include "xc.h" #include "math.h" #include "UART.h" #include "config.h" #include "extern.h" #include "types.h" #include "conv.h" #include "isr.h" #include "CalibrationConstants.h" #include "GainScheduling.h" #include "libq.h" #include "RPM_Calculation.h" #include "WindingCurrentTarget.h" #include "park.h" #include "svgen.h" #include "SendCANPackets.h" #include "limiter.h" #include "PIDGainScaling.h" #include "DAC.h" void WindingCurrentControl(void) { static int first_time = 1; static int overcurrentCtr = 0; int LimiterActive; int ScaledControlOutput; fractional V1[3]; fractional V2[3]; unsigned int DisableCnts; // iTemp1 = 1; if (CalibrationMode > 0) // No Winding Current Control is performed when in Calibration Mode, Bridge controlled by Calibration Routine. { CalibrationMode = 2; //Indicates This routine has been disable return; } if (first_time) //Compute analog counts corresponding to zero's of engineering units. { ParkParm.qAngle = 0; DisableCnts = DISABLETIME * PWM_FREQ / WINDING_CONTROL_PERIOD; first_time = 0; } if (++SPI_OuterSampleCtr == OUTERFILTERSAMPLES) SPI_OuterSampleCtr = 0; SVGenParm.iPWMPeriod = 2 * P1TPER; ParkParm.qIa = PhaseA_Curr; ParkParm.qIb = PhaseB_Curr; int ElecAngle; #if ENC_CNTPERREV == 4096 if (EncoderStatus == INDEX_PULSE_SEEN) ElecAngle = (POSCNT * 10) % 8192; else ElecAngle = (SlowRotatePosition * 10) % 8192; //ElecAngle counts from 0 to 8191 ParkParm.qAngle = ElecAngle * 8; //0 to 65528 #endif #if ENC_CNTPERREV == 16384 if (EncoderStatus == INDEX_PULSE_SEEN) ElecAngle = POSCNT % 3277; else ElecAngle = SlowRotatePosition% 3277; //ElecAngle counts from 0 to 3276 ParkParm.qAngle = ElecAngle * 20; //0 to 65520 #endif ParkParm.qAngle = ParkParm.qAngle + qAngleOffset; SinCos(); ClarkePark(); QuadratureCurrentSamples[SPI_OuterSampleCtr] = ParkParm.qIq; FluxCurrentSamples[SPI_OuterSampleCtr] = ParkParm.qId; //overcurrent detection on magnitude of [Iq Id] V1[0] = ParkParm.qIq; V2[0] = ParkParm.qIq; V1[1] = ParkParm.qId; V2[1] = ParkParm.qId; V1[2] = DisableCurrent; V2[2] = -DisableCurrent; static int RPMSignAtShutdown = 0; VBusSamples[ADC_OuterSampleCtr]; // ?? What does this possibly do? if(P1OVDCON == 0x4000) // { if( (((RPMs >= 0) && (RPMSignAtShutdown == -1)) || ((RPMs < 0) && (RPMSignAtShutdown == 1))) && VBusSamples[ADC_OuterSampleCtr] < AbsoluteMaxTargetVoltage) { StatusBitFields.OverCurrentShutdown = 0; StatusBitFields.OverVoltageShutdown = 0; P1OVDCON = 0x7f00; //Channels 1-3 are in complementary PWM mode } } // if ((_Q15abs(ParkParm.qIq)>DisableCurrent)||(_Q15abs( ParkParm.qId)>DisableCurrent)) if ((_Q15abs(PhaseA_Curr) > DisableCurrent) || (_Q15abs(PhaseB_Curr) > DisableCurrent) || (VBusSamples[ADC_OuterSampleCtr] > ShutdownVoltage)) { overcurrentCtr++; if (overcurrentCtr > DisableCnts) { //to the shutdown disable PWM's P1OVDCON = 0x4000; //Disable Bridge FETS, diode bridge become a rectifier, Load Dump continues to maintain voltage. overcurrentCtr = DisableCnts; StatusBitFields.TorqueCmdLimited = 1; //Indicate commanded torque is not possible. if(RPMs > 0) RPMSignAtShutdown = 1; else RPMSignAtShutdown = -1; if(VBusSamples[ADC_OuterSampleCtr] > ShutdownVoltage) //Redundant comparisons, hopefully the compiler will optimize... StatusBitFields.OverVoltageShutdown = 1; if((_Q15abs(PhaseA_Curr) > DisableCurrent) || (_Q15abs(PhaseB_Curr) > DisableCurrent)) StatusBitFields.OverCurrentShutdown = 1; } } else overcurrentCtr = 0; //Reset counter, hopefully just noise static int firstPower = 1; #define BRIDGE_DELAY 100 //10ms delay at 10,000hz static int BridgeEnableDelay = BRIDGE_DELAY; static int BridgeDisableDelay = 0; if(((_Q15abs(CommandedQuadratureCurrent) < BridgeDisableCurrent) //Disable Bridge if target current magnitude is less than specified value and RPMs are low && (_Q15abs(RPMs) < BridgeDisableRPM)) || (EncoderStatus != INDEX_PULSE_SEEN) || (RPMsValid == 0)) { P1OVDCON = 0x0000; // Turn off FETS firstPower = 1; if(BridgeDisableDelay == 0) { BRIDGE_ENABLE = 0; AUXIO_PIN2 = 0; BridgeEnableDelay = BRIDGE_DELAY; } else BridgeDisableDelay--; PIDInit(&Iq_PID); PIDInit(&Id_PID); //Zero these control loops, integrator term is estimated from RPMs and VBus right before enabling bridge PIDInit(&VqVdLim_PID); //IWindReduction_VqVd_Mag = 0; TargetQuadratureCurrent = CommandedQuadratureCurrent; //Copy this over just so that the TargetCurrent reporting values continue to track Commanded Current, just a convenience return; } else{ if((firstPower == 1)) { long tempControlOutput; tempControlOutput = -(((long) VoltageConstant)*RPMs/VBusSamples[ADC_OuterSampleCtr]); if(tempControlOutput < -(PRELIMITER_TARGETCIRCLERADIUS - RE_ENABLE_HYSTERIS)) tempControlOutput = -(PRELIMITER_TARGETCIRCLERADIUS - RE_ENABLE_HYSTERIS); if(tempControlOutput > (PRELIMITER_TARGETCIRCLERADIUS - RE_ENABLE_HYSTERIS)) tempControlOutput = (PRELIMITER_TARGETCIRCLERADIUS - RE_ENABLE_HYSTERIS); Iq_PID.controlOutput = _Q15neg((int) tempControlOutput); //Set to expected value at start-up RPM/Voltage BRIDGE_ENABLE = 1; AUXIO_PIN2 = 1; if(BridgeEnableDelay == 0) { P1OVDCON = 0x7f00; //Channels 1-3 are in complementary PWM mode firstPower = 0; BridgeDisableDelay = BRIDGE_DELAY; } else BridgeEnableDelay--; } } //Implement Battery Draw Current Limit. This reduces the magnitude of the winding current to satisfy the battery current draw limit. if(StatusBitFields.BattSwitchSetting == 0) //If Batt switch is off, limit draw current to minimum to avoid overheating body-diode of battery switch FET BattDrawILim_PID.controlReference = MaxBattDrawCurrent.UpperLimit; //Specified as the negative current allowable that is closest to zero i.e. -2A. else BattDrawILim_PID.controlReference = MaxBattDrawCurrent.Value; //Specified as a maximum negative current allowable. BattDrawILim_PID.measuredOutput = IBattSamples[ADC_OuterSampleCtr]; //Negative numbers indicate current is being drawn PID(&BattDrawILim_PID); //Positive command signal indicates a desired reduction of winding current magnitude. IWindReduction_BattDraw_Mag = ScaleGainsAndLimitOutput(&BattDrawILim_PID, &BattDrawILim_GSAL); //Apply Gain Scaling and Actuator limits. LowerLimit = 0, UpperLimit = 32767 , set in init.c //This will only ever modify TargetQuadratureCurrent in "motor mode" when current is being drawn from the battery. And there's not need to reduce beyond zero. if (CommandedQuadratureCurrent >= 0) { TargetQuadratureCurrent = _Q15sub(CommandedQuadratureCurrent, IWindReduction_BattDraw_Mag); if (TargetQuadratureCurrent < 0) TargetQuadratureCurrent = 0; } if (CommandedQuadratureCurrent < 0) { TargetQuadratureCurrent = _Q15add(CommandedQuadratureCurrent, IWindReduction_BattDraw_Mag); if (TargetQuadratureCurrent > 0) TargetQuadratureCurrent = 0; } //IWindReduction_VqVd_Mag is the magnitude of the Winding Current correction to keep VqVd at the target circle in motor mode at or above the "rectification speed". // Ultimately this will need to push the system into generating at high RPMs to avoid the no-go zone of the torque-speed plane where there is insufficient bus voltage if (RPMs >= 0) { TargetQuadratureCurrent = _Q15sub(TargetQuadratureCurrent, IWindReduction_VqVd_Mag); if (TargetQuadratureCurrent < _Q15neg(MaxWindCurrent)) TargetQuadratureCurrent = _Q15neg(MaxWindCurrent); } if (RPMs < 0) { TargetQuadratureCurrent = _Q15add(TargetQuadratureCurrent, IWindReduction_VqVd_Mag); if (TargetQuadratureCurrent > MaxWindCurrent) TargetQuadratureCurrent = MaxWindCurrent; } //The above two calculations of TargetQuadratureCurrent should pick the more restrictive value. i.e. they operate in parrall, code could probably be better... //Note in status bit if torque is limited if (TargetQuadratureCurrent != CommandedQuadratureCurrent) //Torque Limitation imposed. StatusBitFields.TorqueCmdLimited = 1; //Winding Quadrature Current Control Loop Iq_PID.controlReference = TargetQuadratureCurrent; Iq_PID.measuredOutput = ParkParm.qIq; PID(&Iq_PID); ParkParm.qVq = _Q15neg(Iq_PID.controlOutput); //Winding Flux Current Control Loop Id_PID.controlReference = TargetFluxCurrent; Id_PID.measuredOutput = ParkParm.qId; PID(&Id_PID); ParkParm.qVd = _Q15neg(Id_PID.controlOutput); V1[0] = ParkParm.qVq; //Store Vq and Vd before they are potentially modified by "limiter" for use in VqVd control loop below. V2[0] = ParkParm.qVq; V1[1] = ParkParm.qVd; V2[1] = ParkParm.qVd; V1[2] = TargetCircleRadius; V2[2] = -TargetCircleRadius; //In the case that Vq/Vd is specified outside the unit circle, //limiter() adjusts Vq/Vd back to the closest point on the unit circle. //This is necessary because the InvPark routine requires the magnitude of Vq/Vd to be less than 1 //In fact this should rarely happen in generator mode because the "VoltageModification_Limiter" control loop will raise //the target voltage just enough to keep the magnitude of Vq/Vd to a slightly smaller number. // In motor mode, VoltageModification_Limiter isn't effective or appropriate, so this VqVd will hit this limit. LimiterActive = 0; LimiterActive = limiter(&(ParkParm.qVd), &(ParkParm.qVq)); //iTemp3 = LimiterActive; if (LimiterActive == 1) { Iq_PID.controlOutput = _Q15neg(ParkParm.qVq); Id_PID.controlOutput = _Q15neg(ParkParm.qVd); } //iTemp1 = Id_PID.controlOutput; iTemp2 = Iq_PID.controlOutput; //Everything is computed for new Bridge Duty Cycles, so set those InvPark(); CalcRefVec(); CalcSVGen(); /* Now, control bus voltage, there are a few factors that affect the target voltage, these are evaluated and then the shunt regulator * control loop is executed and the Load Dump DC is set. By now, IBatt and VBus need to have been measured for this cycle * * The following affects the bus voltage target current. * - If VqVd is exceeds the target circle diameter, the target voltage must be raised to keep it at that target circle. * This applies if the system is generating power (generating) and at some point the batteries must be turned off to keep too much current * from flowing to the batteries. Windup limiters on ... must then be applied. If the system is using power (motoring), the bus voltage target has no authority over the VqVd position so * nothing can be done and the motor torque will be speed limited, in this case windup limiters need to be applied too... * - A control loop on battery current also can reduce the target bus voltage to keep the battery current to within limits. */ if (StatusBitFields.BattSwitchSetting == 1) //Only implement battery charger current limit control loop if batteries are on. Conflict with the need to raise the target voltage for VqVd is resolved by only running this loop if the batteries are on. { /***** Implement Battery Charge Current Limit ***************************************/ /* To reduce the current flowing to the battery, the technique is to reduce the bus voltage */ BattChargeILim_PID.controlReference = MaxBattChargeCurrent.Value; BattChargeILim_PID.measuredOutput = IBattSamples[ADC_OuterSampleCtr]; PID(&BattChargeILim_PID); VoltageModification_BattCurrent = ScaleGainsAndLimitOutput(&BattChargeILim_PID, &BattChargeILim_GSAL); } else VoltageModification_BattCurrent = 0; //Not sure if this is needed, might be better to just let it freeze when batteries get turned off, depends on if one waits for RPMs to go very low before turning switches on. In that case this makes sense /////Compute how close Vq/Vd point is to the maximum allowable magnitude, Positive values of DistanceFromTargetCircle indicate VqVd is inside target circle. //raise target voltage just enough to keep Vq/Vd from crossing the specified magnitude. DistanceFromTargetCircle = -(VectorDotProduct(3, V1, V2) >> 1); //Extra shift to the right required b/c DSP routine only shifts 15 bits down, not sure why. //iTemp1 = DistanceFromTargetCircle; // if (DistanceFromTargetCircle > iTemp2) // iTemp2 = DistanceFromTargetCircle; //Record maximums //VqVd Distance from target circle control loop VqVdLim_PID.controlReference = 0; //Target is to keep VqVd magnitude at zero when needed. VqVdLim_PID.measuredOutput = DistanceFromTargetCircle; //Negative Values means Vq/Vd is outside the target circle radius and VoltageModification_Limiter needs to grow. PID(&VqVdLim_PID); //Positive errors lead to positive command which raises target voltage fractional VqVd_ActuatorCmd; VqVd_ActuatorCmd = ScaleGainsAndLimitOutput(&VqVdLim_PID, &VqVdLim_GSAL); //Use this fcn to update PID loop variables appropriately //Apply the actuatorCmd for Vq/Vd as follows. First, raise the target voltage until the Max is reached. If the max target voltage is reached, the problem is it's in motor mode so move the winding current into the generating quadrant. VoltageModification_Limiter = VqVd_ActuatorCmd; if(ShuntReg_PID.controlReference == AbsoluteMaxTargetVoltage) IWindReduction_VqVd_Mag = _Q15sub(VqVd_ActuatorCmd,_Q15sub(AbsoluteMaxTargetVoltage,_Q15add(TargetVoltage.Value, VoltageModification_BattCurrent))); if(IWindReduction_VqVd_Mag < 0) IWindReduction_VqVd_Mag = 0; //Limit to >= zero adjustment. // if ((DistanceFromTargetCircle < 0) && (IBattSamples[ADC_OuterSampleCtr] > MaxBattChargeCurrent.Value)) //VqVd is limited so voltage will rise, necessitates turning off batteries. : (BattSwitchControl will turn this back on after RPM drops.{ if (DistanceFromTargetCircle < 0) //VqVd is limited so voltage will rise, necessitates turning off batteries. : (BattSwitchControl will turn this back on after RPM drops.{ { BATTERY_SLOW = 0; StatusBitFields.SlowSwitchSetting = 0; BATTERY_SWITCH = 0; StatusBitFields.BattSwitchSetting = 0; } //Implement Shunt Regulator for Voltage Limit ShuntReg_PID.controlReference = _Q15add(TargetVoltage.Value, VoltageModification_BattCurrent); //Should saturate at 0 and 32767 ShuntReg_PID.controlReference = _Q15add(ShuntReg_PID.controlReference, VoltageModification_Limiter); //Should saturate at 0 and 32767 if (ShuntReg_PID.controlReference > AbsoluteMaxTargetVoltage) //Limit target voltage to specified absolute max. ShuntReg_PID.controlReference = AbsoluteMaxTargetVoltage; if (ShuntReg_PID.controlReference < TargetVoltage.LowerLimit) //Limit target voltage to specified minimum. ShuntReg_PID.controlReference = TargetVoltage.LowerLimit; ShuntReg_PID.measuredOutput = VBusSamples[ADC_OuterSampleCtr]; //Use 20kHz measurement as feedback PID(&ShuntReg_PID); ScaledControlOutput = ScaleGainsAndLimitOutput(&ShuntReg_PID, &ShuntReg_GSAL); //Apply Gain Scaling and Actuator limits. The shunt regulator can only pull the voltage down so control output limited to -100% to 0 so GSAL->LowerLimit = -32768 and GSAL->UpperLimt = 0, set in init.c P1DC4 = _Q15neg(ScaledControlOutput) >> 4; //Convert 0 -> -32768 to 0 -> 2*P1TPER. LoadDCSamples[SPI_OuterSampleCtr] = P1DC4; //Record #ifdef DEBUG SetupDACOutputVariables(); //This puts specified data into the SPI buffers to get shifted out to the DACs for debugging on the next SPI cycle. #endif return; }