diff --git a/Matlab/LabDisplay/PostProcEffMap.m b/Matlab/LabDisplay/PostProcEffMap.m index bcdc67e..fd35ebc 100644 --- a/Matlab/LabDisplay/PostProcEffMap.m +++ b/Matlab/LabDisplay/PostProcEffMap.m @@ -1,8 +1,8 @@ close all clear all -filenm = 'May17-2021_EffMapp_PosToNegTorque_Scale1'; -%filenm = 'Mar22-2021_EffMapp_AveragedQuadrants'; +%filenm = 'May17-2021_EffMapp_PosToNegTorque_Scale1'; +filenm = 'Mar22-2021_EffMapp_AveragedQuadrants'; load([filenm '.mat']); diff --git a/PowerConverter.X/CmdInterpreter.c b/PowerConverter.X/CmdInterpreter.c index 948c5b4..3f3b517 100644 --- a/PowerConverter.X/CmdInterpreter.c +++ b/PowerConverter.X/CmdInterpreter.c @@ -20,6 +20,8 @@ void SetRPM_StdDevTarget(unsigned char *RcvdBytes); //CmdID = 28 void SetMaxBattDrawCurrent(unsigned char *RcvdBytes); //CmdID = 29 void SetBiasWindCurrent(unsigned char *RcvdBytes); //CmdID = 34 void SetCANPacketRate(unsigned char *RcvdBytes); //CmdID = 35 +void SetPermissiveMode(unsigned char *RcvdBytes); //CmdID = 36 + //void Reset(unsigned char *RcvdBytes); //CmdID = 255 @@ -36,7 +38,8 @@ ReceivedCmdParams CmdParams[NUMBER_OF_CMDS_TO_INTERPRET] = { {28, SetRPM_StdDevTarget}, {29, SetMaxBattDrawCurrent}, {34, SetBiasWindCurrent}, - {35, SetCANPacketRate} //, + {35, SetCANPacketRate}, + {36, SetPermissiveMode}//, // {255, Reset} }; @@ -249,6 +252,19 @@ void SetCANPacketRate(unsigned char *RcvdBytes) { } +/*SetPermissiveMode: CmdID = 36 + * Input: takes single from CANbus (two bytes). + * Scaling: None + * Valid Values: 0 = Off, 1 = On + * Out of Range Behavior: Only sets if argument is 0 or 1 + */ +void SetPermissiveMode(unsigned char *RcvdBytes) +{ + if ((RcvdBytes[1] >= 0) && (RcvdBytes[1] <= 1)) + StatusBitFields.PermissiveMode = RcvdBytes[1]; + +} + /*Reset (No Argument) CmdID = 255 */ //void Reset(unsigned char *RcvdBytes) { diff --git a/PowerConverter.X/WindingCurrentLimits.c b/PowerConverter.X/WindingCurrentLimits.c index 6e64b90..585482d 100644 --- a/PowerConverter.X/WindingCurrentLimits.c +++ b/PowerConverter.X/WindingCurrentLimits.c @@ -109,7 +109,31 @@ fractional CurrentMinMax(fractional f_N, fractional locTargetCurrent) /* Returns first_time = 0; } - N = _Q15abs(f_N); + + SC_Range_Max = SC_RANGE_MAX*CRSF_POSITION; + SC_Range_Min = SC_RANGE_MIN*CRSF_POSITION; + StopRange = STOP_RANGE*CRSF_POSITION; + StopGain = ConvertEngUnitsToFract(5000, RPMs_cnt, RPMs_eng) / StopRange; //RPMcnts per Range cnts + + int N_adjust = 0; + + if ((StatusBitFields.SpringRangeValid == 1) && (StatusBitFields.PermissiveMode == 0)) { //Valid Spring Range and not in permissive mode, adjust RPM for current winding limit computation. + + if ((f_N > 0) && (SC_Range < (SC_Range_Min + StopRange))) + { + N_adjust = ((SC_Range_Min + StopRange) - SC_Range) * StopGain; + } + if (N_adjust > MaxRPMAdjustment) + N_adjust = MaxRPMAdjustment; + + if ((f_N < 0) && (SC_Range > (SC_Range_Max - StopRange))) + N_adjust = ((SC_Range_Max - StopRange) - SC_Range) * StopGain; + if(N_adjust < -MaxRPMAdjustment) + N_adjust = -MaxRPMAdjustment; + } + + + N = _Q15abs(f_N+N_adjust); if (N >= N_cnts[LO_TABLE_LENGTH - 1]) { @@ -181,4 +205,37 @@ void TestCurrentMinMax() { } } + +void TestCurrentMinMaxWithStop() { + + fractional Ibig, Ismall; + fractional f_N = 0; + char s[256]; + + Ibig = ConvertEngUnitsToFract(MAX_WINDCURRENTLIMIT, IWind_cnt, IWind_eng); //It's a mystery why this computes differently here than in CurrentMinMax + Ismall = ConvertEngUnitsToFract(-MAX_WINDCURRENTLIMIT, IWind_cnt, IWind_eng); + //sprintf(s,"Ibig/Ismall = %d %d \r\n",Ibig,Ismall); + //putsU1(s); + Ibig = 32767; + Ismall = -32768; + + float range; + for (f_N = ConvertEngUnitsToFract(7000, RPMs_cnt, RPMs_eng); f_N >= ConvertEngUnitsToFract(0, RPMs_cnt, RPMs_eng); f_N -= ConvertEngUnitsToFract(100, RPMs_cnt, RPMs_eng)) { + + for(range = 82.0;range>-2;range = range-.25) + { + SC_Range = range*CRSF_POSITION; + SC_RangeCtr = SC_RANGE_VALID_TIMEOUT; + StatusBitFields.SpringRangeValid = 1; //Indicate SC_Range is valid in mode word. + StatusBitFields.PermissiveMode = 0; //Indicate not in permissive mode is valid in mode word. + sprintf(s, "%f ", + ConvertFractToEngUnits(CurrentMinMax(f_N, Ibig), IWind_cnt, IWind_eng) + ); + putsU1(s); + } + sprintf(s, "\r\n"); putsU1(s); + } + +} + #endif \ No newline at end of file diff --git a/PowerConverter.X/WindingCurrentLimits.h b/PowerConverter.X/WindingCurrentLimits.h index 558cf4e..6e5c2ab 100644 --- a/PowerConverter.X/WindingCurrentLimits.h +++ b/PowerConverter.X/WindingCurrentLimits.h @@ -7,6 +7,7 @@ fractional CurrentMinMax(fractional f_N, fractional locTargetCurrent); #ifdef DEBUG void TestCurrentMinMax(); +void TestCurrentMinMaxWithStop(); #endif #endif /* WINDINGCURRENTLIMITS_H */ diff --git a/PowerConverter.X/config.h b/PowerConverter.X/config.h index f4c07ed..cead208 100644 --- a/PowerConverter.X/config.h +++ b/PowerConverter.X/config.h @@ -31,10 +31,11 @@ #define BRIDGE_DISABLE_RPM 0 //Make "window" infinately narrow for cal mode #endif + #define PRELIMITER_TARGETCIRCLERADIUS 31000 #define RE_ENABLE_HYSTERIS 2000 //Distance inside targetcircle radius VqVd Magnitude must be to re-enable battery switch. -#define NUMBER_OF_CMDS_TO_INTERPRET 11 +#define NUMBER_OF_CMDS_TO_INTERPRET 12 @@ -102,7 +103,7 @@ #define MAX_RPM_STD_DEV_TARGET 2000 //In RPM #define DEFAULT_BIASCURRENT 0.0 //Start with zero bias current -#define MAX_BIASCURRENT 15.0 //Maximum allowable winding bias current Magnitude that can be applied. +#define MAX_BIASCURRENT 20.0 //Maximum allowable winding bias current Magnitude that can be applied. #define MAX_WINDCURRENTLIMIT 35 //Winding Current Limit, Amps. Limit on internal target @@ -175,6 +176,14 @@ #define CRSF_PERCENTAGES 100 //cnts per 100 percent #define CRSF_PRESSURE 1000 //cnts per psi #define CRSF_TORQUE 500 //cnts per N-m +#define CRSF_POSITION 10 //cnts per inch + +#define SPRINGCONTROLLER_CAN_SRCID 1 +#define SC_RANGE_VALID_TIMEOUT 150 //milli-seconds. //The ~1kHz interrupt counts ths down, so times out in ~150ms... +#define SC_RANGE_MIN 0 //Inches +#define SC_RANGE_MAX 80 //Inches +#define STOP_RANGE 10 //Inches from SC_RANGE_MIN and SC_RANGE_MAX to increase generator torque +#define MAX_RPM_ADJUSTMENT 7000 //Maximum amount to modify RPM in determining WindingCurrentLimit near ends of stroke. #define CONTROLLER_SN 2 // 0 = Generation 1 Test Board diff --git a/PowerConverter.X/extern.c b/PowerConverter.X/extern.c index 13344d2..750c4c2 100644 --- a/PowerConverter.X/extern.c +++ b/PowerConverter.X/extern.c @@ -24,6 +24,14 @@ volatile int I2C_Data[64]; volatile int I2C_NumDataBytes; volatile int I2C_data_ctr; +volatile signed int SC_Range; +volatile unsigned int SC_RangeCtr = 0; +volatile signed int SC_Range_Max; +volatile signed int SC_Range_Min; +volatile signed int StopRange; +volatile signed int StopGain; +volatile signed int MaxRPMAdjustment; + volatile tParkParm ParkParm; volatile tSVGenParm SVGenParm; diff --git a/PowerConverter.X/extern.h b/PowerConverter.X/extern.h index 6872bd5..d2ae3f9 100644 --- a/PowerConverter.X/extern.h +++ b/PowerConverter.X/extern.h @@ -24,6 +24,14 @@ extern volatile int I2C_Data[64]; extern volatile int I2C_NumDataBytes; extern volatile int I2C_data_ctr; +extern volatile unsigned int SC_Range; +extern volatile unsigned int SC_RangeCtr; +extern volatile signed int SC_Range_Max; +extern volatile signed int SC_Range_Min; +extern volatile signed int StopRange; +extern volatile signed int StopGain; +extern volatile signed int MaxRPMAdjustment; + #ifdef DEBUG extern tPID *PIDptr; //Pointer to PID structure for gain adjustments in UARTRxInterrupt.c extern volatile fractional *kCoeffs; //Pointer to kCoeffs arrah for gain adjustments in UARTRxInterrupt.c diff --git a/PowerConverter.X/init.c b/PowerConverter.X/init.c index 4620b99..2c9ac9e 100644 --- a/PowerConverter.X/init.c +++ b/PowerConverter.X/init.c @@ -178,6 +178,12 @@ void InitVariables() VoltageConstant = 32767*VOLTAGE_CONSTANT*(ConvertEngUnitsToFract(1, VBus_cnt, VBus_eng)-ConvertEngUnitsToFract(0, VBus_cnt, VBus_eng))/(ConvertEngUnitsToFract(1,RPMs_cnt,RPMs_eng)-ConvertEngUnitsToFract(0,RPMs_cnt,RPMs_eng)); + SC_Range_Max = SC_RANGE_MAX*CRSF_POSITION; + SC_Range_Min = SC_RANGE_MIN*CRSF_POSITION; + StopRange = STOP_RANGE*CRSF_POSITION; + MaxRPMAdjustment = ConvertEngUnitsToFract(MAX_RPM_ADJUSTMENT, RPMs_cnt,RPMs_eng); + StopGain = MaxRPMAdjustment/StopRange; //RPMcnts per Range cnts + Status = 0; @@ -420,6 +426,39 @@ C1BUFPNT1bits.F0BP = 0x8; C1FEN1bits.FLTEN0=0x1; + +/* Select Acceptance Filter Mask 1 for Acceptance Filter 1, This accepts the packet that includes the spring controller range measurement bits */ +C1FMSKSEL1bits.F1MSK=0x01; +/* Configure Acceptance Filter Mask 1 register to care about all bits except bottom nine of EID which is the sequence number. (bits set to 0 here don't count. +Mask Bits (29-bits) : 0b0 0000 0000 0000 0000 0001 1111 1111 +SID<10:0> : 0b111 1111 1111 ..SID<10:0> or EID<28:18> +EID<17:16> : 0b11 +..EID<17:16> +EID<15:0> : 0b1111 1111 1111 1111 ..EID<15:0> */ +C1RXM1SIDbits.SID = (0x1FFC0000) >> 18; +C1RXM1SIDbits.EID = (0x00030000) >> 16; +C1RXM1EIDbits.EID = (0x0000FE00); //Ignore bottom nine bits for the sequence number since this packet is coming from the battery controller + +/* Configure Acceptance Filter 0 to match extended identifier, Controller ID = from #define and CMD ID = dec(00) to correspond to data packet with range in it from the spring converter, and any sequence number. +Filter Bits (29-bits) : 0b0 0000 0000 0000 0000 000x xxxx xxxx +SID<10:0> : 0b00000000000 ..SID<10:0> or EID<28:18> +EID<17:16> : 0b00 ..EID<17:16> +EID<15:0> : 0b0000000xxxxxxxxx ..EID<15:0> */ +tmp_EID = Assemble_packetID(SPRINGCONTROLLER_CAN_SRCID, 00, 0); +C1RXF1SIDbits.SID = (tmp_EID & 0x1FFC0000) >> 18; +C1RXF1SIDbits.EID = (tmp_EID & 0x00030000) >> 16; +C1RXF1EIDbits.EID = (tmp_EID & 0x0000FE00); + +/* Acceptance Filter 1 to check for Extended Identifier */ +C1RXM1SIDbits.MIDE = 0x1; +C1RXF1SIDbits.EXIDE= 0x1; + +/* Acceptance Filter 1 to use Message Buffer 9 to store message */ +C1BUFPNT1bits.F1BP = 0x9; +/* Enable Acceptance Filter 1 */ +C1FEN1bits.FLTEN1=0x1; + + //THis code sets up acceptance filter for data packets from the battery controller. Not used in this version so commented out. ///* Select Acceptance Filter Mask 1 for Acceptance Filter 1, This accepts the packet that includes the stop-charge bits */ //C1FMSKSEL1bits.F1MSK=0x01; @@ -473,7 +512,7 @@ CAN_ENABLE = 1; } void InitDma12() //DMA channel 1 is used to transfer data to the CAN controller for subsequent transmission - //DMA channel 2 is used to transfer recieved data from the CAN controller to the CPU memory + //DMA channel 2 is used to transfer received data from the CAN controller to the CPU memory { /* Initialize the DMA channel 0 for ECAN TX and clear the colission flags */ diff --git a/PowerConverter.X/isr.c b/PowerConverter.X/isr.c index 978b5c6..4eac4b2 100644 --- a/PowerConverter.X/isr.c +++ b/PowerConverter.X/isr.c @@ -246,7 +246,6 @@ void __attribute__((interrupt, no_auto_psv)) _T1Interrupt(void) //PR1 is set to #ifdef DEBUG f_time = f_time + 1.0 / CAN_packet_rate.Value; #endif - //Form data for sending out over CAN by further filtering data output from 1kHz filter #ifdef FOO fractional outSamples[REPORTINGFILTERSAMPLES]; @@ -333,6 +332,7 @@ void __attribute__((interrupt, no_auto_psv)) _C1Interrupt(void) { __asm__ volatile ("push CORCON"); + if (C1INTFbits.RBIF) /* check to see if the interrupt is caused by receive */ { /*check to see if buffer 8 is full, indicating CmdID was 10 */ if (C1RXFUL1bits.RXFUL8) { @@ -346,6 +346,23 @@ void __attribute__((interrupt, no_auto_psv)) _C1Interrupt(void) { C1RXFUL1bits.RXFUL8 = 0; } + if (C1RXFUL1bits.RXFUL9) + { /*check to see if buffer 9 is full, indicating CmdID was 0 and the packet contains the "range" measurement from the spring controller*/ + DisAssembleCAN_Packet(9, &RcvdMessage); + + + + if(RcvdMessage.data_length >= 8) //There should be 8 bytes in a valid message + { + SC_Range = 256*RcvdMessage.data[3]+RcvdMessage.data[2]; + SC_RangeCtr = SC_RANGE_VALID_TIMEOUT; //Reset counter on reception of valid range from string controller. + } +//#ifdef DEBUG //Only compile this code in DEBUG mode (set in config.h) + // char s[256]; + // sprintf(s,"Buf9 %d \r\n",256*RcvdMessage.data[1]+RcvdMessage.data[0]); putsU1(s); +//#endif + C1RXFUL1bits.RXFUL9 = 0; + } /* clear flag */ C1INTFbits.RBIF = 0; @@ -606,6 +623,19 @@ void __attribute__((interrupt, no_auto_psv)) _T5Interrupt(void) __asm__ volatile ("push CORCON"); + + if (SC_RangeCtr > 0) //Keep from rolling down below zero. + { + SC_RangeCtr--; + StatusBitFields.SpringRangeValid = 1; //Indicate SC_Range is valid in mode word. + } + else + { + SC_RangeCtr = 0; + StatusBitFields.SpringRangeValid = 0; //Indicate SC_Range has become invalid in mode word. + } + + if((P1DC4 > 0x0600) && (SEVTCMP == 0x0400)) SEVTCMP = 0x0000; if((P1DC4 < 0x0200) && (SEVTCMP == 0x0000)) @@ -660,9 +690,11 @@ void __attribute__((interrupt, no_auto_psv)) _T5Interrupt(void) AuxTorque[ReportingFilterCtr] = outSamples[OUTERFILTERSAMPLES - 1]; #endif - //Implement Mode Timeouts + //Implement Mode Timeouts and lockouts if SC_Range is invalid. if (EncoderStatus == INDEX_PULSE_SEEN) - if (TorqueCmdTimeout_ctr > 0) //Revert to Generator mode if no torque or velocity command has been received within timeout time. + { + //Revert to Generator mode if no torque or velocity command has been received within timeout time + if ((TorqueCmdTimeout_ctr > 0) ) { if(--TorqueCmdTimeout_ctr == 0) StatusBitFields.Mode = GENERATOR_MODE; @@ -674,6 +706,15 @@ void __attribute__((interrupt, no_auto_psv)) _T5Interrupt(void) BiasCurrent.Value = 0; } + if ((StatusBitFields.PermissiveMode == 0) && (StatusBitFields.SpringRangeValid == 0)) //Could be logically combined above, but left separate for clarity. + { + StatusBitFields.Mode = GENERATOR_MODE; //Force to Generator Mode + BiasCurrent.Value = 0; //Remove bias Current to prevent motoring. + TorqueCmdTimeout_ctr = 0; //Reset timeouts to zero. Means a single invalid SC_Range instance causes Torque Cmd and BiasCommand to need to be restarted. + BiasCmdTimeout_ctr = 0; + } + } + /***** Set Target Torque Based on Mode **********************/ switch (StatusBitFields.Mode) { @@ -692,7 +733,7 @@ void __attribute__((interrupt, no_auto_psv)) _T5Interrupt(void) break; case TORQUE_MODE: //CommandedCurrent is specified by user command, so just need to add specified bias current - CommandedCurrent = UserCommandedCurrent.Value; //_Q15add(UserCommandedCurrent.Value,BiasCurrent.Value); //Sum in Bias Current. + CommandedCurrent = UserCommandedCurrent.Value; break; } diff --git a/PowerConverter.X/main.c b/PowerConverter.X/main.c index 23f0266..839a57e 100644 --- a/PowerConverter.X/main.c +++ b/PowerConverter.X/main.c @@ -80,6 +80,12 @@ int main(void) { VREF_ENABLE = 1; //Turn on External Reference InitVariables(); + + + InitU1(); //RS-232 + TestCurrentMinMaxWithStop(); + while(1); + InitPIDGains(); //init TMR1 to interrupt at rate specified in config.h @@ -121,6 +127,7 @@ int main(void) { InitU1(); //RS-232 #endif + //Set up DMA channel and ADC InitDma0(); //Argument is number of samples to transfer per interrupt. InitDma3(); //Argument is number of samples to transfer per interrupt. @@ -189,6 +196,9 @@ int main(void) { // PrintScaling(); // TestCurrentMinMax(); #endif + + + while (1) { #ifdef DEBUG if (f_time > last_time) //Wait for time to be updated. @@ -219,7 +229,7 @@ int main(void) { } - sprintf(s, "%.3f %s %7.1f RPM %5.2f A %5.2f A %5.2f A %5.2f A %5.2f A %5.3f A %5.2f A %5.2f A %6.1f V %8.3f V %6.3f A %6.1f V %6.1f V %5d - %5d %5d %5d %5d %5d %6.3f %8.5f %6.3f %x-%d %d %d - %d %d - %d %d %d %6.3f PSI %6.3f N-m %f %x %x %f %d %ld %f\r\n", + sprintf(s, "%.3f %s %7.1f RPM %5.2f A %5.2f A %5.2f A %5.2f A %5.2f A %5.3f A %5.2f A %5.2f A %6.1f V %8.3f V %6.3f A %6.1f V %6.1f V %5d - %5d %5d %5d %5d %5d %6.3f %8.5f %6.3f %x-%d %d %d - %d %d - %d %d %d %6.3f PSI %6.3f N-m %f %x %x %f %d %ld %f %d %d %d\r\n", f_time, ModeStr, ConvertFractToEngUnits(RPMs, RPMs_cnt, RPMs_eng), ConvertFractToEngUnits(UserCommandedCurrent.Value, IWind_cnt, IWind_eng), @@ -260,7 +270,8 @@ int main(void) { BRIDGE_ENABLE, SEVTCMP, ConvertFractToEngUnits(GainScheduleRPMStdDevTarget.Value, RPMs_cnt, RPMs_eng), - ScaleIntegrationGain.Value,lTemp1,ConvertFractToEngUnits(TargetVoltage.Value, VBus_cnt, VBus_eng) + ScaleIntegrationGain.Value,lTemp1,ConvertFractToEngUnits(TargetVoltage.Value, VBus_cnt, VBus_eng), + SC_Range,SC_RangeCtr,StatusBitFields.PermissiveMode ); //// // sprintf(s,"%.3f %d %d %8.5f %8.5f %8.5f\r\n", diff --git a/PowerConverter.X/types.h b/PowerConverter.X/types.h index fb6d9c0..1346054 100644 --- a/PowerConverter.X/types.h +++ b/PowerConverter.X/types.h @@ -25,6 +25,28 @@ typedef union } BATTERY_CONTROLLER_STATUS; +struct SpringControllerStatusBits { + unsigned int ReliefValveRequest : 7; //Request to open/close valve + unsigned int ReliefValveStatus : 1; // Status of Relief valve open/close + unsigned int PumpRequest : 1; // Request to turn pump on or off + unsigned int PumpStatus : 1; // Status of pump switch + unsigned int PumpOverTemp : 1; // Status of pump OverTemp signal + unsigned int PumpToggle : 1; // Status of pump Toggle. + unsigned int TetherPowerRequest : 1; //Request to turn tether power on or off + unsigned int TetherPowerStatus : 1; //Status of tether power relay + unsigned int LR_Fault : 1; //Status of LRF fault input + unsigned int AUX_Fault : 1; //Status of AUX fault input +}; + +typedef union +{ + unsigned int status; + struct SpringControllerStatusBits bits; +} SPRING_CONTROLLER_STATUS; + + + + struct PowerConverterStatusBits { unsigned int Mode : 2; //controller mode. specified by defines in config.h (GENERATOR_MODE, TORQUE_MODE) unsigned int TorqueCmdLimited : 1; //defines what is limited the Torque Command (0 = Not limited, 1 = Rate, 2 = Min, 3 = Max). @@ -35,6 +57,9 @@ struct PowerConverterStatusBits { unsigned int GainScheduleMode : 1; // 0 = off. 1 = on. unsigned int OverCurrentShutdown : 1; //1 indicates drive is in overcurrent shutdown. unsigned int OverVoltageShutdown : 1; //1 indicates drive is in overvoltage shutdown. + unsigned int SpringRangeValid : 1; //1 indicates the current SC_Range value is valid (received within approximately SC_RANGE_VALID_TIMEOUT milliseconds) + unsigned int PermissiveMode : 1; //1 indicates user has selected "permissive mode" which allows WindingCurrent commands to be set even if SpringController Range packets aren't arriving + }; typedef union