Index: firmware/App/Controllers/BloodFlow.c =================================================================== diff -u -rf7e3018ec6ab762fe08efb42b21fb2ca970174b0 -r9a9d04b84f4345fca87fb14d26f09d497b08aae8 --- firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision f7e3018ec6ab762fe08efb42b21fb2ca970174b0) +++ firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision 9a9d04b84f4345fca87fb14d26f09d497b08aae8) @@ -43,9 +43,9 @@ #define MAX_BLOOD_PUMP_PWM_DUTY_CYCLE 0.88 // controller will error if PWM duty cycle > 90%, so set max to 88% #define MIN_BLOOD_PUMP_PWM_DUTY_CYCLE 0.12 // controller will error if PWM duty cycle < 10%, so set min to 12% -#define BP_CONTROL_INTERVAL ( 500 / TASK_GENERAL_INTERVAL ) // interval (ms/task time) at which the blood pump is controlled -#define BP_P_COEFFICIENT 0.0002 // P term for blood pump control -#define BP_I_COEFFICIENT 0.00002 // I term for blood pump control +#define BP_CONTROL_INTERVAL ( 1000 / TASK_GENERAL_INTERVAL ) // interval (ms/task time) at which the blood pump is controlled +#define BP_P_COEFFICIENT 0.00005 // P term for blood pump control +#define BP_I_COEFFICIENT 0.00015 // I term for blood pump control #define BP_MAX_PWM_DC_DELTA 0.01 // prevents large steps in PWM duty cycle while controlling #define BP_MIN_PWM_DC_DELTA -0.01 @@ -60,7 +60,7 @@ #define BP_REV_PER_LITER 124.0 // rotor revolutions per liter #define BP_ML_PER_MIN_TO_PUMP_RPM_FACTOR ( BP_REV_PER_LITER / ML_PER_LITER ) #define BP_GEAR_RATIO 32.0 // blood pump motor to blood pump gear ratio -#define BP_MOTOR_RPM_TO_PWM_DC_FACTOR 0.0003717 // ~27 BP motor RPM = 1% PWM duty cycle +#define BP_MOTOR_RPM_TO_PWM_DC_FACTOR 0.00042 // ~24 BP motor RPM = 1% PWM duty cycle #define BP_PWM_ZERO_OFFSET 0.1 // 10% PWM duty cycle = zero speed #define BP_PWM_FROM_ML_PER_MIN(rate) ( (rate) * BP_ML_PER_MIN_TO_PUMP_RPM_FACTOR * BP_GEAR_RATIO * BP_MOTOR_RPM_TO_PWM_DC_FACTOR + BP_PWM_ZERO_OFFSET ) @@ -69,7 +69,7 @@ #define SIGN_FROM_12_BIT_VALUE(v) ( (S16)(v) - (S16)BLOODPUMP_ADC_MID_PT_BITS ) #define BLOOD_FLOW_SAMPLE_FREQ ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) -#define SIZE_OF_ROLLING_AVG (U32)( (F32)BLOOD_FLOW_SAMPLE_FREQ * 0.8 ) // measured blood flow is filtered w/ moving average +#define SIZE_OF_ROLLING_AVG ( BLOOD_FLOW_SAMPLE_FREQ * 2 ) // measured blood flow is filtered w/ moving average #define MAX_FLOW_FILTER_INTERVAL 5 // slowest sample interval for filter is every 5th sample typedef enum BloodPump_States @@ -135,6 +135,7 @@ static BLOOD_PUMP_STATE_T handleBloodPumpRampingUpState( void ); static BLOOD_PUMP_STATE_T handleBloodPumpRampingDownState( void ); static BLOOD_PUMP_STATE_T handleBloodPumpControlToTargetState( void ); +static void setBloodPumpControlSignalPWM( F32 newPWM ); static void stopBloodPump( void ); static void releaseBloodPumpStop( void ); static void setBloodPumpDirection( MOTOR_DIR_T dir ); @@ -338,7 +339,7 @@ { // set initial PWM duty cycle bloodPumpPWMDutyCyclePctSet = BP_PWM_ZERO_OFFSET + MAX_BLOOD_PUMP_PWM_STEP_CHANGE; - etpwmSetCmpA( etpwmREG1, (U32)( (S32)( ( bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) ); + setBloodPumpControlSignalPWM( bloodPumpPWMDutyCyclePctSet ); // allow blood pump to run in requested direction setBloodPumpDirection( bloodPumpDirection ); releaseBloodPumpStop(); @@ -368,7 +369,7 @@ { // start ramp down to stop bloodPumpPWMDutyCyclePctSet -= MAX_BLOOD_PUMP_PWM_STEP_CHANGE; - etpwmSetCmpA( etpwmREG1, (U32)( (S32)( ( bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) ); + setBloodPumpControlSignalPWM( bloodPumpPWMDutyCyclePctSet ); result = BLOOD_PUMP_RAMPING_DOWN_STATE; } // have we reached end of ramp up? @@ -382,7 +383,7 @@ else { bloodPumpPWMDutyCyclePctSet += MAX_BLOOD_PUMP_PWM_STEP_CHANGE; - etpwmSetCmpA( etpwmREG1, (U32)( (S32)( ( bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) ); + setBloodPumpControlSignalPWM( bloodPumpPWMDutyCyclePctSet ); } return result; @@ -419,7 +420,7 @@ else { bloodPumpPWMDutyCyclePctSet -= MAX_BLOOD_PUMP_PWM_STEP_CHANGE; - etpwmSetCmpA( etpwmREG1, (U32)( (S32)( ( bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) ); + setBloodPumpControlSignalPWM( bloodPumpPWMDutyCyclePctSet ); } return result; @@ -447,14 +448,29 @@ { newPWM = runPIController( PI_CONTROLLER_ID_BLOOD_FLOW, tgtFlow, actFlow ); bloodPumpPWMDutyCyclePctSet = newPWM; - etpwmSetCmpA( etpwmREG1, (U32)( (S32)( ( bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) ); + setBloodPumpControlSignalPWM( newPWM ); bpControlTimerCounter = 0; } return result; } /************************************************************************* + * @brief setBloodPumpControlSignalPWM + * The setBloodPumpControlSignalPWM function sets the PWM duty cycle for \n + * the blood pump to a given %. + * @details + * Inputs : none + * Outputs : blood pump stop signal activated, PWM duty cycle zeroed + * @param newPWM : new duty cycle % to apply to PWM + * @return none + *************************************************************************/ +static void setBloodPumpControlSignalPWM( F32 newPWM ) +{ + etpwmSetCmpA( etpwmREG1, (U32)( (S32)( ( newPWM * (F32)(etpwmREG1->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) ); +} + +/************************************************************************* * @brief stopBloodPump * The stopBloodPump function sets the blood pump stop signal. * @details @@ -622,13 +638,13 @@ F32 measMCSpd = getMeasuredBloodPumpMCSpeed(); F32 measMCCurr = getMeasuredBloodPumpMCCurrent(); F32 pumpPWMPctDutyCycle = bloodPumpPWMDutyCyclePctSet * FRACTION_TO_PERCENT_FACTOR; -#ifdef DEBUG_ENABLED - // TODO - temporary debug code - remove later - char debugFlowStr[ 256 ]; - - sprintf( debugFlowStr, "Blood Set Pt.:%5d, Meas. Flow:%5d, Speed:%5d RPM, Current:%5d mA, PWM:%5d \n", flowStPt, (S32)measFlow, (S32)measMCSpd, (S32)measMCCurr, (S32)pumpPWMPctDutyCycle ); - sendDebugData( (U08*)debugFlowStr, strlen(debugFlowStr) ); -#endif +//#ifdef DEBUG_ENABLED +// // TODO - temporary debug code - remove later +// char debugFlowStr[ 256 ]; +// +// sprintf( debugFlowStr, "Blood Set Pt.:%5d, Meas. Flow:%5d, Speed:%5d RPM, Current:%5d mA, PWM:%5d \n", flowStPt, (S32)measFlow, (S32)measMCSpd, (S32)measMCCurr, (S32)pumpPWMPctDutyCycle ); +// sendDebugData( (U08*)debugFlowStr, strlen(debugFlowStr) ); +//#endif broadcastBloodFlowData( flowStPt, measFlow, measRotSpd, measSpd, measMCSpd, measMCCurr, pumpPWMPctDutyCycle ); bloodFlowDataPublicationTimerCounter = 0; } @@ -725,6 +741,22 @@ } flowReadingsTmrCtr = INC_WRAP( flowReadingsTmrCtr, 0, MAX_FLOW_FILTER_INTERVAL - 1 ); +#ifdef DEBUG_ENABLED + { + // TODO - temporary debug code - remove later +// char debugFlowStr[ 40 ]; + //S32 num = (S32)(flow); + //S32 dec = (S32)(fabs(flow-(S32)(flow))*100.0); +// S32 numf = (S32)(measuredBloodFlowRate.data); +// S32 decf = (S32)(fabs(measuredBloodFlowRate.data-(S32)(measuredBloodFlowRate.data))*100.0); +// S32 nump = (S32)bloodPumpPWMDutyCyclePctSet; +// S32 decp = (S32)((bloodPumpPWMDutyCyclePctSet-(S32)bloodPumpPWMDutyCyclePctSet)*100.0); + +// sprintf( debugFlowStr, "%5d.%02d %5d.%02d\n", numf, decf, numf, decf ); +// sprintf( debugFlowStr, "%5d.%02d %5d.%02d\n", num, dec, numf, decf ); +// sendDebugData( (U08*)debugFlowStr, strlen(debugFlowStr) ); + } +#endif } /*************************************************************************