template <class dataType, class floatType> void Pid<dataType, floatType>::Init(
dataType kp,
dataType ki,
dataType kd,
ctrlDir_t controllerDir,
outputMode_t outputMode,
floatType samplePeriodMs,
dataType minOutput,
dataType maxOutput,
dataType setPoint)
{
SetOutputLimits(minOutput, maxOutput);
this->samplePeriodMs = samplePeriodMs;
SetControllerDirection(controllerDir);
this->outputMode = outputMode;
// Set tunings with provided constants
SetTunings(kp, ki, kd);
this->setPoint = setPoint;
prevInput = 0;
prevOutput = 0;
pTerm = floatType(0.0);
iTerm = floatType(0.0);
dTerm = floatType(0.0);
}
template <class dataType, class floatType> void Pid<dataType, floatType>::SetSamplePeriod(floatType newSamplePeriodMs)
{
if (newSamplePeriodMs > 0)
{
dataType ratio = (dataType)newSamplePeriodMs
/ floatType(samplePeriodMs);
Zi *= ratio;
Zd /= ratio;
samplePeriodMs = newSamplePeriodMs;
}
}
void updateFPURegs(RegisterViewModel& model, const State& state) {
for(std::size_t i=0;i<MAX_FPU_REGS_COUNT;++i)
{
const auto reg=state.fpu_register(i);
const auto comment = floatType(reg)==FloatValueClass::PseudoDenormal ?
QObject::tr("pseudo-denormal") : "";
model.updateFPUReg(i,reg,comment);
}
model.updateFCR(state.fpu_control_word());
const auto fsr=state.fpu_status_word();
model.updateFSR(fsr,FSRComment(fsr));
model.updateFTR(state.fpu_tag_word());
{
const Register FIS=state["FIS"];
if(FIS) model.updateFIS(FIS.value<edb::value16>());
else model.invalidateFIS();
}
{
const Register FDS=state["FDS"];
if(FDS) model.updateFDS(FDS.value<edb::value16>());
else model.invalidateFDS();
}
{
const Register FIP=state["FIP"];
if(FIP.bitSize()==64)
model.updateFIP(FIP.value<edb::value64>());
else if(FIP.bitSize()==32)
model.updateFIP(FIP.value<edb::value32>());
else
model.invalidateFIP();
}
{
const Register FDP=state["FDP"];
if(FDP.bitSize()==64)
model.updateFDP(FDP.value<edb::value64>());
else if(FDP.bitSize()==32)
model.updateFDP(FDP.value<edb::value32>());
else
model.invalidateFDP();
}
{
const Register FOP=state["fopcode"];
if(FOP) {
const auto value=FOP.value<edb::value16>();
// Yes, FOP is a big-endian view of the instruction
const auto comment=value>0x7ff ? QString("?!!") :
QObject::tr("Insn: %1 %2").arg((edb::value8(value>>8)+0xd8).toHexString()).arg(edb::value8(value).toHexString());
model.updateFOP(value,comment);
}
else model.invalidateFOP();
}
template <class dataType, class floatType> void Pid<dataType, floatType>::SetTunings(dataType kp, dataType ki, dataType kd)
{
if (kp<0 || ki<0 || kd<0)
return;
actualKp = kp;
actualKi = ki;
actualKd = kd;
// Calculate time-step-scaled PID terms
Zp = kp;
// The next bit requires floatType->dataType casting functionality.
Zi = ki * (dataType)(samplePeriodMs/floatType(1000.0));
Zd = kd / (dataType)(samplePeriodMs/floatType(1000.0));
if(controllerDir == PID_REVERSE)
{
Zp = (0 - Zp);
Zi = (0 - Zi);
Zd = (0 - Zd);
}
#if(pidPRINT_DEBUG == 1)
snprintf(debugBuff,
sizeof(debugBuff),
"PID: Tuning parameters set. Kp = %.1f, Ki = %.1f, Kd = %f.1, "
"Zp = %.1f, Zi = %.1f, Zd = %.1f, with sample period = %.1fms\r\n",
actualKp,
actualKi,
actualKd,
Zp,
Zi,
Zd,
samplePeriodMs);
PrintDebug(debugBuff);
#endif
}
