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; }