public double GetParam(Ctrl ctrl, PID.Param param)
{
double ans = double.MaxValue;
lock(this.pidMutex)
{
switch(ctrl)
{
case Ctrl.THROTTLE:
if(this.throttlePid != null)
ans = this.throttlePid.GetParam(param);
break;
case Ctrl.ROLL:
if(this.rollPid != null)
ans = this.rollPid.GetParam(param);
break;
case Ctrl.PITCH:
if(this.pitchPid != null)
ans = this.pitchPid.GetParam(param);
break;
case Ctrl.YAW:
if(this.yawPid != null)
ans = this.yawPid.GetParam(param);
break;
}
}
return ans;
}
private void SetInfo()
{
StreamReader sr = new StreamReader("PidConfig.txt");
string line = null;
int ch, offset, minVal, maxVal, meanVal;
double spanFactor, imuTs = -1, adcTs = -1, kp, ki, kd, refValue;
while((line = sr.ReadLine()) != null)
{
if(line.StartsWith("#") || line == "")
continue;
string[] words = line.Split(new char[]{' ', '\t'}, StringSplitOptions.RemoveEmptyEntries);
switch(words[0])
{
case "imuTs":
imuTs = double.Parse(words[1]);
break;
case "adcTs":
adcTs = double.Parse(words[1]);
break;
case "roll":
kp = double.Parse(words[1]);
ki = double.Parse(words[2]);
kd = double.Parse(words[3]);
ch = int.Parse(words[4]);
offset = int.Parse(words[5]);
spanFactor = double.Parse(words[6]);
minVal = int.Parse(words[7]);
maxVal = int.Parse(words[8]);
meanVal = int.Parse(words[9]);
refValue = double.Parse(words[10]);
lock(this.pidMutex)
{
this.rollPid = new PID(imuTs, kp, ki, kd, offset, spanFactor, minVal, maxVal, meanVal, refValue, false, false);
this.pids[ch-1] = this.rollPid;
}
break;
case "pitch":
kp = double.Parse(words[1]);
ki = double.Parse(words[2]);
kd = double.Parse(words[3]);
ch = int.Parse(words[4]);
offset = int.Parse(words[5]);
spanFactor = double.Parse(words[6]);
minVal = int.Parse(words[7]);
maxVal = int.Parse(words[8]);
meanVal = int.Parse(words[9]);
refValue = double.Parse(words[10]);
lock(this.pidMutex)
{
this.pitchPid = new PID(imuTs, kp, ki, kd, offset, spanFactor, minVal, maxVal, meanVal, refValue, false, false);
this.pids[ch-1] = this.pitchPid;
}
break;
case "yaw":
kp = double.Parse(words[1]);
ki = double.Parse(words[2]);
kd = double.Parse(words[3]);
ch = int.Parse(words[4]);
offset = int.Parse(words[5]);
spanFactor = double.Parse(words[6]);
minVal = int.Parse(words[7]);
maxVal = int.Parse(words[8]);
meanVal = int.Parse(words[9]);
refValue = double.Parse(words[10]);
lock(this.pidMutex)
{
this.yawPid = new PID(imuTs, kp, ki, kd, offset, spanFactor, minVal, maxVal, meanVal, refValue, true, false);
this.pids[ch-1] = this.yawPid;
}
break;
case "throttle":
kp = double.Parse(words[1]);
ki = double.Parse(words[2]);
kd = double.Parse(words[3]);
ch = int.Parse(words[4]);
offset = int.Parse(words[5]);
spanFactor = double.Parse(words[6]);
minVal = int.Parse(words[7]);
maxVal = int.Parse(words[8]);
meanVal = int.Parse(words[9]);
refValue = double.Parse(words[10]);
lock(this.pidMutex)
{
this.throttlePid = new PID(adcTs, kp, ki, kd, offset, spanFactor,minVal,maxVal,meanVal, refValue, false,true);
this.pids[ch-1] = this.throttlePid;
}
break;
}
}
}
public bool RefreshParam(Ctrl ctrl, PID.Param param, double val)
{
bool ok = false;
lock(this.pidMutex)
{
switch(ctrl)
{
case Ctrl.THROTTLE:
if(this.throttlePid != null)
ok = this.throttlePid.RefreshParam(param, val);
break;
case Ctrl.ROLL:
if(this.rollPid != null)
ok = this.rollPid.RefreshParam(param, val);
break;
case Ctrl.PITCH:
if(this.pitchPid != null)
ok = this.pitchPid.RefreshParam(param, val);
break;
case Ctrl.YAW:
if(this.yawPid != null)
ok = this.yawPid.RefreshParam(param, val);
break;
}
}
return ok;
}