Index: PIDControllers.c =================================================================== diff -u -rce319db25846ee9f64cbf6d3eb7b2c9aae75a3e5 -ra0f051c3f707ecbc537bdc85c40f0df346066652 --- PIDControllers.c (.../PIDControllers.c) (revision ce319db25846ee9f64cbf6d3eb7b2c9aae75a3e5) +++ PIDControllers.c (.../PIDControllers.c) (revision a0f051c3f707ecbc537bdc85c40f0df346066652) @@ -51,12 +51,11 @@ F32 referenceSignal; ///< Reference signal. F32 measuredSignal; ///< Measured signal. F32 errorSignal; ///< Reference - measured signal. - F32 errorSumBeforeWindUp; ///< Error signal before windup correction. - F32 errorSum; ///< Error integral after windup correction. + F32 errorSum; ///< Error integral. F32 errorPrevious; ///< Previous error signal. F32 controlSignal; ///< Actual control signal. F32 maxErrorSumStep; ///< Maximum change in I (error sum) for a single control interval. - PID_CONTROLLER_DIRECTIONS_T direction; ///< PI controller control direction. + PID_CONTROLLER_DIRECTIONS_T direction; ///< PI controller control direction. BOOL isFeedForwardEnabled; ///< Eanble or disable Feed forward term F32 feedForward; ///< Feed forward term } PID_CONTROLLER_T; @@ -65,12 +64,12 @@ /// PID Controllers - initial configurations. static PID_CONTROLLER_T pidControllers[ NUM_OF_PID_CONTROLLERS_IDS ] = -{ // Kp Ki Kd uMax uMin ref meas err esw esum eprev ctrl Ilimit controller type, feedfwdEnabled, feedfwdValue +{ // Kp Ki Kd uMax uMin ref meas err esum eprev ctrl Ilimit controller type, feedfwdEnabled, feedfwdValue #ifdef _TD_ - { 0.0, 0.0, 0.0, 0.90, 0.10, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 100.0, CONTROLLER_UNIDIRECTIONAL, FALSE, 0.0 }, // PI_CONTROLLER_ID_BLOOD_FLOW + { 0.0, 0.0, 0.0, 0.9, 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, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 25.0, CONTROLLER_UNIDIRECTIONAL, FALSE, 0.0 }, // PID_CONTROLLER_ID_RO_PUMP + { 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 25.0, CONTROLLER_UNIDIRECTIONAL, FALSE, 0.0 }, // PID_CONTROLLER_ID_RO_PUMP #endif }; @@ -121,8 +120,11 @@ else { #ifdef _DD_ - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PID_CTRL_INVALID_CONTROLLER1, (U32)controllerID ) #endif +#ifdef _TD_ + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_PID_CTRL_INVALID_CONTROLLER1, (U32)controllerID ) +#endif } } @@ -147,19 +149,21 @@ { SET_CONTROLLER( controller, controllerID ); controller->controlSignal = RANGE( initialControlSignal, controller->uMin, controller->uMax ); - controller->referenceSignal = 0.0; - controller->errorSignal = 0.0; - controller->errorSum = controller->controlSignal / controller->Ki; - controller->errorSumBeforeWindUp = controller->errorSum; - controller->measuredSignal = 0.0; + controller->referenceSignal = 0.0F; + controller->errorSignal = 0.0F; + controller->errorSum = 0.0F; + controller->measuredSignal = 0.0F; controller->feedForward = feedFowardSignal; - controller->errorPrevious = 0.0; + controller->errorPrevious = 0.0F; } else { #ifdef _DD_ - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PID_CTRL_INVALID_CONTROLLER2, (U32)controllerID ) #endif +#ifdef _DT_ + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_PID_CTRL_INVALID_CONTROLLER2, (U32)controllerID ) +#endif } } @@ -182,9 +186,6 @@ if ( controllerID < NUM_OF_PID_CONTROLLERS_IDS ) { - F32 controlSignalBeforeWindup; - F32 windupError; - SET_CONTROLLER( controller, controllerID ); controller->referenceSignal = referenceSignal; @@ -218,56 +219,34 @@ controller->errorSum += controller->errorSignal; } - // Calculate control signal from i term - controller->errorSumBeforeWindUp = controller->errorSum; - controlSignalBeforeWindup = ( controller->Ki * controller->errorSum ); + // TODO Redo anti windup by enforcing a max |error sum| - if ( FALSE == controller->isFeedForwardEnabled ) + // If feed forward enabled, use new feed forward value as basis for control signal instead of previous control signal + if ( FALSE != controller->isFeedForwardEnabled ) { - controller->controlSignal = RANGE( controlSignalBeforeWindup, controller->uMin, controller->uMax ); + controller->controlSignal = controller->feedForward; } - 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 + // Update control signal + controller->controlSignal += ( controller->Ki * controller->errorSum ); controller->controlSignal += ( controller->Kp * controller->errorSignal ); + controller->controlSignal += ( controller->Kd * ( controller->errorSignal - controller->errorPrevious ) ); + // Record error signal for D term next time + controller->errorPrevious = controller->errorSignal; - // Add d term to control signal - if ( controller->Kd != 0 ) - { - // Wait for previous error to be set before adding d term - if ( controller->errorPrevious != 0 ) - { - controller->controlSignal += ( controller->Kd * ( controller->errorSignal - controller->errorPrevious ) ); - } - controller->errorPrevious = controller->errorSignal; - } - - // Add feed forward term to control signal - if ( FALSE != controller->isFeedForwardEnabled ) - { - controller->controlSignal = controller->controlSignal + controller->feedForward; - } - - //Re-apply range limits + // Apply range limits to new control signal controller->controlSignal = RANGE( controller->controlSignal, controller->uMin, controller->uMax ); result = controller->controlSignal; } else { #ifdef _DD_ - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PID_CTRL_INVALID_CONTROLLER3, (U32)controllerID ) #endif +#ifdef _TD_ + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_PID_CTRL_INVALID_CONTROLLER3, (U32)controllerID ) +#endif } return result; @@ -310,10 +289,6 @@ break; case CONTROLLER_SIGNAL_PID_ERROR_SUM: - output = controller->errorSumBeforeWindUp; - break; - - case CONTROLLER_SIGNAL_PID_ERROR_SUM_AFTER_WINDUP: output = controller->errorSum; break; @@ -342,16 +317,22 @@ default: #ifdef _DD_ - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_SIGNAL, (U32)signalID ) + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PID_CTRL_INVALID_SIGNAL, (U32)signalID ) #endif +#ifdef _TD_ + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_PID_CTRL_INVALID_SIGNAL, (U32)signalID ) +#endif break; } // End of switch } else { // Invalid controller given #ifdef _DD_ - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PID_CTRL_INVALID_CONTROLLER4, (U32)controllerID ) #endif +#ifdef _TD_ + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_PID_CTRL_INVALID_CONTROLLER4, (U32)controllerID ) +#endif } return output; Index: PIDControllers.h =================================================================== diff -u -rce319db25846ee9f64cbf6d3eb7b2c9aae75a3e5 -ra0f051c3f707ecbc537bdc85c40f0df346066652 --- PIDControllers.h (.../PIDControllers.h) (revision ce319db25846ee9f64cbf6d3eb7b2c9aae75a3e5) +++ PIDControllers.h (.../PIDControllers.h) (revision a0f051c3f707ecbc537bdc85c40f0df346066652) @@ -50,8 +50,7 @@ CONTROLLER_SIGNAL_PID_REFERENCE = 0, ///< Reference value CONTROLLER_SIGNAL_PID_MEASURED, ///< Measured value CONTROLLER_SIGNAL_PID_ERROR, ///< Error value - CONTROLLER_SIGNAL_PID_ERROR_SUM, ///< Error sum before anti-windup - CONTROLLER_SIGNAL_PID_ERROR_SUM_AFTER_WINDUP, ///< Error sum after anti-windup + CONTROLLER_SIGNAL_PID_ERROR_SUM, ///< Error sum CONTROLLER_SIGNAL_PID_ERROR_PREVIOUS, ///< Previous error signal CONTROLLER_SIGNAL_PID_PROPORTIONAL_OUTPUT, ///< P portion of controller output signal CONTROLLER_SIGNAL_PID_INTEGRAL_OUTPUT, ///< I portion of controller output signal