185 lines
6.6 KiB
C
185 lines
6.6 KiB
C
/**
|
|
* @file pressureController.c
|
|
*
|
|
* @copyright Nehemis SARL reserves all rights even in the event of industrial
|
|
* property rights. We reserve all rights of disposal such as
|
|
* copying and passing on to third parties.
|
|
*
|
|
* @brief
|
|
*
|
|
*/
|
|
|
|
|
|
/******************************************************************************
|
|
* Include Header Files
|
|
******************************************************************************/
|
|
#include "pressureController.h"
|
|
#include "math.h"
|
|
/******************************************************************************
|
|
* Macro constant declarations
|
|
******************************************************************************/
|
|
|
|
/******************************************************************************
|
|
* Type declarations
|
|
******************************************************************************/
|
|
|
|
/******************************************************************************
|
|
* Module Global Variable Declarations
|
|
******************************************************************************/
|
|
|
|
/******************************************************************************
|
|
* Private Function Declarations
|
|
******************************************************************************/
|
|
|
|
/******************************************************************************
|
|
* Extern Function Declarations
|
|
******************************************************************************/
|
|
void PuControllersInitInputs(PuControllersPidInput_st *pumpPin_pst)
|
|
{
|
|
/* === Pump RO Pressure === */
|
|
pumpPin_pst->kpGain_f32 = 6.0f;
|
|
pumpPin_pst->kiGain_f32 = 5.0f;
|
|
pumpPin_pst->kdGain_f32 = -3.0f;
|
|
pumpPin_pst->satDeriv_f32 = 5.0f;
|
|
pumpPin_pst->sampleTime_f32 = 0.1f;
|
|
pumpPin_pst->commandMin_f32 = 0.0f;
|
|
pumpPin_pst->commandMax_f32 = 100.0f;
|
|
pumpPin_pst->regulatorActivation_f32 = 1.0f;
|
|
pumpPin_pst->integralResetFlag_b = false;
|
|
}
|
|
|
|
|
|
void PuControllersPidLoop(const PuControllersPidInput_st *input_pst, PuControllersPidOutput_st *output_pst, PuControllersPidState_st *state_pst)
|
|
{
|
|
float32 derivTerm_f32;
|
|
|
|
/* External Trigger */
|
|
if (input_pst->integralResetFlag_b)
|
|
{
|
|
state_pst->integState_f32 = 0.0f;
|
|
}
|
|
if(input_pst->regulatorActivation_f32 == 0.0f)
|
|
{
|
|
state_pst->prevMeasFilt2_f32 = 0.0f;
|
|
state_pst->sumOutput_f32 = 0.0f;
|
|
state_pst->integState_f32 = 0.0f;
|
|
|
|
output_pst->derivOutput_f32 = 0.0f;
|
|
output_pst->integOutput_f32 = 0.0f;
|
|
output_pst->propOutput_f32 = 0.0f;
|
|
|
|
}
|
|
/* Check if the regulator control should be active */
|
|
if (input_pst->regulatorActivation_f32 > 0.0f)
|
|
{
|
|
/* If regulator was inactive previously, reset integral and derivative states */
|
|
if (!state_pst->isRegulatorActive_b)
|
|
{
|
|
state_pst->prevMeasFilt2_f32 = 0.0f;
|
|
state_pst->integState_f32 = 0.0f;
|
|
state_pst->isRegulatorActive_b = true;
|
|
}
|
|
|
|
/* Compute derivative component based on measurement change rate */
|
|
derivTerm_f32 = -(input_pst->measFilt2_f32 - state_pst->prevMeasFilt2_f32) * input_pst->kdGain_f32 / input_pst->sampleTime_f32;
|
|
|
|
/* Apply saturation limits to derivative output to prevent excessive corrections */
|
|
if (derivTerm_f32 > input_pst->satDeriv_f32)
|
|
{
|
|
output_pst->derivOutput_f32 = input_pst->satDeriv_f32;
|
|
}
|
|
else if (derivTerm_f32 < -input_pst->satDeriv_f32)
|
|
{
|
|
output_pst->derivOutput_f32 = -input_pst->satDeriv_f32;
|
|
}
|
|
else
|
|
{
|
|
output_pst->derivOutput_f32 = derivTerm_f32;
|
|
}
|
|
|
|
/* Output current integral state */
|
|
output_pst->integOutput_f32 = state_pst->integState_f32;
|
|
|
|
/* Compute proportional output based on setpoint error */
|
|
derivTerm_f32 = input_pst->setpoint_f32 - input_pst->measFilt_f32;
|
|
|
|
if (fabsf(derivTerm_f32) > input_pst->tolerance_f32)
|
|
{
|
|
output_pst->propOutput_f32 = input_pst->kpGain_f32 * derivTerm_f32;
|
|
}
|
|
else
|
|
{
|
|
output_pst->propOutput_f32 = 0.0f;
|
|
output_pst->derivOutput_f32 = 0.0;
|
|
}
|
|
/* Sum PID components to form the overall regulator output */
|
|
state_pst->sumOutput_f32 = output_pst->propOutput_f32 + output_pst->integOutput_f32 + output_pst->derivOutput_f32;
|
|
|
|
/* Calculate integral contribution based on error */
|
|
derivTerm_f32 *= input_pst->kiGain_f32;
|
|
|
|
/* Adjust integral contribution to respect command saturation limits */
|
|
if (state_pst->prevCommandInput_f32 >= input_pst->commandMax_f32)
|
|
{
|
|
derivTerm_f32 = (float32)(fmin((float64)derivTerm_f32, 0.0f));
|
|
}
|
|
else if (input_pst->commandMin_f32 >= state_pst->prevCommandInput_f32)
|
|
{
|
|
derivTerm_f32 = (float32)(fmax(0.0f, (float64)derivTerm_f32));
|
|
}
|
|
|
|
/* Update previous measurement for derivative calculation in next cycle */
|
|
state_pst->prevMeasFilt2_f32 = input_pst->measFilt2_f32;
|
|
|
|
if (input_pst->kpGain_f32 == 0.0f)// I controller only
|
|
{
|
|
state_pst->integState_f32 += input_pst->sampleTime_f32 * derivTerm_f32;
|
|
}
|
|
/* Integrate integral term with adjusted contribution */
|
|
else
|
|
{
|
|
if (fabsf(output_pst->propOutput_f32 / input_pst->kpGain_f32) > input_pst->tolerance_f32)
|
|
{
|
|
state_pst->integState_f32 += input_pst->sampleTime_f32 * derivTerm_f32;
|
|
}
|
|
}
|
|
|
|
}
|
|
else if (state_pst->isRegulatorActive_b)
|
|
{
|
|
/* Disable regulator and reset summed output if regulator is deactivated */
|
|
state_pst->sumOutput_f32 = 0.0f;
|
|
state_pst->isRegulatorActive_b = false;
|
|
}
|
|
|
|
/* Combine regulator output with feed-forward command to form total command */
|
|
derivTerm_f32 = input_pst->commandRm_f32 + state_pst->sumOutput_f32;
|
|
|
|
/* Ensure final command output respects specified saturation limits */
|
|
if (derivTerm_f32 > input_pst->commandMax_f32)
|
|
{
|
|
output_pst->commandSetpoint_f32 = input_pst->commandMax_f32;
|
|
}
|
|
else if (derivTerm_f32 < input_pst->commandMin_f32)
|
|
{
|
|
output_pst->commandSetpoint_f32 = input_pst->commandMin_f32;
|
|
}
|
|
else
|
|
{
|
|
output_pst->commandSetpoint_f32 = derivTerm_f32;
|
|
}
|
|
|
|
/* Store last command to aid future saturation checks */
|
|
state_pst->prevCommandInput_f32 = derivTerm_f32;
|
|
}
|
|
|
|
|
|
/******************************************************************************
|
|
* Private Function Definitions
|
|
******************************************************************************/
|
|
|
|
/******************************************************************************
|
|
* Callback Function Definitions
|
|
******************************************************************************/
|
|
|