Index: PIControllers.c =================================================================== diff -u -r64f7cb9eed55338a584084d7cb934df96818af03 -re36314e77be984668ded42e1af9aefc54b60c677 --- PIControllers.c (.../PIControllers.c) (revision 64f7cb9eed55338a584084d7cb934df96818af03) +++ PIControllers.c (.../PIControllers.c) (revision e36314e77be984668ded42e1af9aefc54b60c677) @@ -55,22 +55,29 @@ F32 controlSignal; ///< Actual control signal. F32 maxErrorSumStep; ///< Maximum change in I (error sum) for a single control interval. PI_CONTROLLER_DIRECTIONS_T direction; ///< PI controller control direction. + BOOL isFeedForwardEnabled; ///< Eanble or disable Feed forward term + F32 feedForward; ///< Feed forward term } PI_CONTROLLER_T; // ********** private data ********** /// PI Controllers - initial configurations. static PI_CONTROLLER_T piControllers[ NUM_OF_PI_CONTROLLERS_IDS ] = -{ // Kp Ki uMax uMin ref meas err esw esum ctrl Ilimit controller type +{ // Kp Ki uMax uMin ref meas err esw esum ctrl Ilimit controller type, feedfwdEnabled, feedfwdValue +#ifdef _TD_ + { 0.0, 0.0, 0.90, 0.10, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 100.0, CONTROLLER_UNIDIRECTIONAL, FALSE, 0.0 }, // PI_CONTROLLER_ID_BLOOD_FLOW +#endif #ifdef _DD_ - { 0.0, 0.0, 2650.0, 350.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, CONTROLLER_BIDIRECTIONAL }, // PI_CONTROLLER_ID_D12_PUMP - { 0.0, 0.0, 2650.0, 350.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, CONTROLLER_BIDIRECTIONAL }, // PI_CONTROLLER_ID_D48_PUMP - { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, CONTROLLER_BIDIRECTIONAL }, // PI_CONTROLLER_ID_D5_HEAT - { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, CONTROLLER_BIDIRECTIONAL }, // PI_CONTROLLER_ID_D45_HEAT + { 0.0, 0.0, 2650, 350, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, CONTROLLER_BIDIRECTIONAL, FALSE, 0.0 }, // PI_CONTROLLER_ID_D12_PUMP + { 0.0, 0.0, 2650, 350, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, CONTROLLER_BIDIRECTIONAL, FALSE, 0.0 }, // PI_CONTROLLER_ID_D48_PUMP + { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 15.0, CONTROLLER_BIDIRECTIONAL, TRUE, 0.0 }, // PI_CONTROLLER_ID_D5_HEAT + { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 5.0, CONTROLLER_BIDIRECTIONAL, FALSE, 0.0 }, // PI_CONTROLLER_ID_D45_HEAT #endif #ifdef _RO_ - { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, CONTROLLER_UNIDIRECTIONAL }, // PI_CONTROLLER_ID_RO_PUMP - { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, CONTROLLER_UNIDIRECTIONAL }, // PI_CONTROLLER_ID_RO_PUMP_MAX_PRES + { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, CONTROLLER_UNIDIRECTIONAL, FALSE, 0.0 }, // PI_CONTROLLER_ID_RO_PUMP + { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, CONTROLLER_UNIDIRECTIONAL, FALSE, 0.0 }, // PI_CONTROLLER_ID_RO_PUMP_MAX_PRES + { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, CONTROLLER_UNIDIRECTIONAL, FALSE, 0.0 }, // PI_CONTROLLER_ID_BOOST_PUMP_FLOW + { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, CONTROLLER_UNIDIRECTIONAL, FALSE, 0.0 }, // PI_CONTROLLER_ID_BOOST_PUMP_PRES #endif }; @@ -88,10 +95,13 @@ * @param kI Coefficient for integral * @param controlMin Minimum control output * @param controlMax Maximum control output + * @param isFeedForwardEnabled Feed forward enabled if true, otherwise not. + * @param feedFowardSignal feedforward value to be applied to the control output * @return none *************************************************************************/ void initializePIController( PI_CONTROLLER_ID_T controllerID, F32 initialControlSignal, - F32 kP, F32 kI, F32 controlMin, F32 controlMax ) + F32 kP, F32 kI, F32 controlMin, F32 controlMax, + BOOL isFeedForwardEnabled, F32 feedFowardSignal ) { if ( controllerID < NUM_OF_PI_CONTROLLERS_IDS ) { @@ -110,15 +120,16 @@ } controller->uMin = controlMin; controller->uMax = controlMax; - resetPIController( controllerID, initialControlSignal ); + controller->isFeedForwardEnabled = isFeedForwardEnabled; + resetPIController( controllerID, initialControlSignal, feedFowardSignal ); } else { #ifdef _DD_ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) #endif #ifdef _RO_ - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_RO_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_FP_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) #endif } } @@ -133,9 +144,10 @@ * Id is passed. * @param controllerID ID filter number * @param initialControlSignal Value of the output on the first iteration + * @param feedFowardSignal Value of feed froward calculation if exists. * @return none *************************************************************************/ -void resetPIController( PI_CONTROLLER_ID_T controllerID, F32 initialControlSignal ) +void resetPIController( PI_CONTROLLER_ID_T controllerID, F32 initialControlSignal, F32 feedFowardSignal ) { PI_CONTROLLER_T *controller; @@ -148,14 +160,15 @@ controller->errorSum = controller->controlSignal / controller->Ki; controller->errorSumBeforeWindUp = controller->errorSum; controller->measuredSignal = 0.0; + controller->feedForward = feedFowardSignal; } else { #ifdef _DD_ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) #endif #ifdef _RO_ - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_RO_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_FP_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) #endif } } @@ -218,17 +231,33 @@ // Calculate control signal from i term controller->errorSumBeforeWindUp = controller->errorSum; controlSignalBeforeWindup = ( controller->Ki * controller->errorSum ); - controller->controlSignal = RANGE( controlSignalBeforeWindup, controller->uMin, controller->uMax ); + if ( FALSE == controller->isFeedForwardEnabled ) + { + controller->controlSignal = RANGE( controlSignalBeforeWindup, controller->uMin, controller->uMax ); + } + else + { + controller->controlSignal = RANGE( controlSignalBeforeWindup, ( controller->uMin - controller->feedForward ), ( controller->uMax - controller->feedForward ) ); + } + // Handle anti-windup for i term windupError = controlSignalBeforeWindup - controller->controlSignal; if ( fabs( windupError ) > NEARLY_ZERO ) { controller->errorSum -= ( windupError / controller->Ki ); } - // Add p term to control signal and re-apply range limits + // Add p term to control signal controller->controlSignal += ( controller->Kp * controller->errorSignal ); + + // Add feed forward term to control signal + if ( FALSE != controller->isFeedForwardEnabled ) + { + controller->controlSignal = controller->controlSignal + controller->feedForward; + } + + //Re-apply range limits controller->controlSignal = RANGE( controller->controlSignal, controller->uMin, controller->uMax ); result = controller->controlSignal; @@ -239,7 +268,7 @@ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) #endif #ifdef _RO_ - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_RO_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_FP_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) #endif } @@ -298,6 +327,10 @@ output = controller->Ki * controller->errorSum; break; + case CONTROLLER_SIGNAL_FEEDFORWARD_OUTPUT: + output = controller->feedForward; + break; + case CONTROLLER_SIGNAL_CONTROL: output = controller->controlSignal; break; @@ -307,7 +340,7 @@ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_SIGNAL, (U32)signalID ) #endif #ifdef _RO_ - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_RO_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_SIGNAL, (U32)signalID ) + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_FP_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_SIGNAL, (U32)signalID ) #endif break; } // End of switch @@ -318,7 +351,7 @@ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) #endif #ifdef _RO_ - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_RO_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_FP_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) #endif } @@ -355,7 +388,7 @@ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_STEP_LIMIT, (U32)stepLimit ) #endif #ifdef _RO_ - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_RO_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_STEP_LIMIT, (U32)stepLimit ) + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_FP_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_STEP_LIMIT, (U32)stepLimit ) #endif } @@ -368,7 +401,7 @@ #endif #ifdef _RO_ // Invalid controller given - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_RO_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_FP_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) #endif } }