private static byte PitchCalib(PIDManager pid, ImuEulerMessage m) { byte ans; if (m.pitch.V > 2) { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.PITCH); double span = pid.GetSpanValue(PIDManager.Ctrl.PITCH); int offset = pid.GetOffsetValue(PIDManager.Ctrl.PITCH); Console.WriteLine("MeanValue: " + meanVal); Console.WriteLine("Span: " + span); Console.WriteLine("Offset: " + offset); ans = (byte)(((meanVal + calValuePitch) - offset) / span); statePitch = false; } else if (m.pitch.V < -2) { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.PITCH); double span = pid.GetSpanValue(PIDManager.Ctrl.PITCH); int offset = pid.GetOffsetValue(PIDManager.Ctrl.PITCH); Console.WriteLine("MeanValue: " + meanVal); Console.WriteLine("Span: " + span); Console.WriteLine("Offset: " + offset); ans = (byte)(((meanVal - calValuePitch) - offset) / span); statePitch = true; } else if (!statePitch) { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.PITCH); double span = pid.GetSpanValue(PIDManager.Ctrl.PITCH); int offset = pid.GetOffsetValue(PIDManager.Ctrl.PITCH); Console.WriteLine("MeanValue: " + meanVal); Console.WriteLine("Span: " + span); Console.WriteLine("Offset: " + offset); ans = (byte)(((meanVal + calValuePitch) - offset) / span); } else { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.PITCH); double span = pid.GetSpanValue(PIDManager.Ctrl.PITCH); int offset = pid.GetOffsetValue(PIDManager.Ctrl.PITCH); Console.WriteLine("MeanValue: " + meanVal); Console.WriteLine("Span: " + span); Console.WriteLine("Offset: " + offset); ans = (byte)(((meanVal - calValuePitch) - offset) / span); } return(ans); }
private static byte YawCalib(PIDManager pid, ImuEulerMessage m) { byte ans; Console.WriteLine("INITIAL VAL: " + pid.GetParam(PIDManager.Ctrl.YAW, PID.Param.INITIAL_VAL)); if ((m.yaw.V - pid.GetParam(PIDManager.Ctrl.YAW, PID.Param.INITIAL_VAL)) > 2) { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.YAW); double span = pid.GetSpanValue(PIDManager.Ctrl.YAW); int offset = pid.GetOffsetValue(PIDManager.Ctrl.YAW); ans = (byte)(((meanVal - calValueYaw) - offset) / span); stateYaw = true; } else if (m.yaw.V - pid.GetParam(PIDManager.Ctrl.YAW, PID.Param.INITIAL_VAL) < -2) { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.YAW); double span = pid.GetSpanValue(PIDManager.Ctrl.YAW); int offset = pid.GetOffsetValue(PIDManager.Ctrl.YAW); ans = (byte)(((meanVal + calValueYaw) - offset) / span); stateYaw = false; } else if (!stateYaw) { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.YAW); double span = pid.GetSpanValue(PIDManager.Ctrl.YAW); int offset = pid.GetOffsetValue(PIDManager.Ctrl.YAW); ans = (byte)(((meanVal - calValueYaw) - offset) / span); } else { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.YAW); double span = pid.GetSpanValue(PIDManager.Ctrl.YAW); int offset = pid.GetOffsetValue(PIDManager.Ctrl.YAW); ans = (byte)(((meanVal - calValueYaw) - offset) / span); } return(ans); }
private static int calValueThrottle = 180; //20% span private static byte ThrottleCalib(PIDManager pid, AdcMessage m) { byte ans; if (m.tas > 12) { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.THROTTLE); double span = pid.GetSpanValue(PIDManager.Ctrl.THROTTLE); int offset = pid.GetOffsetValue(PIDManager.Ctrl.THROTTLE); ans = (byte)(((meanVal - calValueThrottle) - offset) / span); stateThrottle = false; } else if (m.tas < 8) { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.THROTTLE); double span = pid.GetSpanValue(PIDManager.Ctrl.THROTTLE); int offset = pid.GetOffsetValue(PIDManager.Ctrl.THROTTLE); ans = (byte)(((meanVal + calValueThrottle) - offset) / span); stateThrottle = true; } else if (!stateThrottle) { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.THROTTLE); double span = pid.GetSpanValue(PIDManager.Ctrl.THROTTLE); int offset = pid.GetOffsetValue(PIDManager.Ctrl.THROTTLE); ans = (byte)(((meanVal - calValueThrottle) - offset) / span); } else { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.THROTTLE); double span = pid.GetSpanValue(PIDManager.Ctrl.THROTTLE); int offset = pid.GetOffsetValue(PIDManager.Ctrl.THROTTLE); ans = (byte)(((meanVal + calValueThrottle) - offset) / span); } return(ans); }
private static byte YawCalib(PIDManager pid, ImuEulerMessage m) { byte ans; Console.WriteLine("INITIAL VAL: " + pid.GetParam(PIDManager.Ctrl.YAW, PID.Param.INITIAL_VAL)); if((m.yaw.V - pid.GetParam(PIDManager.Ctrl.YAW, PID.Param.INITIAL_VAL)) > 2) { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.YAW); double span = pid.GetSpanValue(PIDManager.Ctrl.YAW); int offset = pid.GetOffsetValue(PIDManager.Ctrl.YAW); ans = (byte)(((meanVal - calValueYaw)-offset)/span); stateYaw = true; } else if(m.yaw.V - pid.GetParam(PIDManager.Ctrl.YAW, PID.Param.INITIAL_VAL) < -2) { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.YAW); double span = pid.GetSpanValue(PIDManager.Ctrl.YAW); int offset = pid.GetOffsetValue(PIDManager.Ctrl.YAW); ans = (byte)(((meanVal + calValueYaw)-offset)/span); stateYaw = false; } else if(!stateYaw) { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.YAW); double span = pid.GetSpanValue(PIDManager.Ctrl.YAW); int offset = pid.GetOffsetValue(PIDManager.Ctrl.YAW); ans = (byte)(((meanVal - calValueYaw)-offset)/span); } else { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.YAW); double span = pid.GetSpanValue(PIDManager.Ctrl.YAW); int offset = pid.GetOffsetValue(PIDManager.Ctrl.YAW); ans = (byte)(((meanVal - calValueYaw)-offset)/span); } return ans; }
private static byte PitchCalib(PIDManager pid, ImuEulerMessage m) { byte ans; if(m.pitch.V > 2) { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.PITCH); double span = pid.GetSpanValue(PIDManager.Ctrl.PITCH); int offset = pid.GetOffsetValue(PIDManager.Ctrl.PITCH); Console.WriteLine("MeanValue: " + meanVal); Console.WriteLine("Span: " + span); Console.WriteLine("Offset: " + offset); ans = (byte)(((meanVal + calValuePitch)-offset)/span); statePitch = false; } else if(m.pitch.V < -2) { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.PITCH); double span = pid.GetSpanValue(PIDManager.Ctrl.PITCH); int offset = pid.GetOffsetValue(PIDManager.Ctrl.PITCH); Console.WriteLine("MeanValue: " + meanVal); Console.WriteLine("Span: " + span); Console.WriteLine("Offset: " + offset); ans = (byte)(((meanVal - calValuePitch)-offset)/span); statePitch = true; } else if(!statePitch) { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.PITCH); double span = pid.GetSpanValue(PIDManager.Ctrl.PITCH); int offset = pid.GetOffsetValue(PIDManager.Ctrl.PITCH); Console.WriteLine("MeanValue: " + meanVal); Console.WriteLine("Span: " + span); Console.WriteLine("Offset: " + offset); ans = (byte)(((meanVal + calValuePitch)-offset)/span); } else { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.PITCH); double span = pid.GetSpanValue(PIDManager.Ctrl.PITCH); int offset = pid.GetOffsetValue(PIDManager.Ctrl.PITCH); Console.WriteLine("MeanValue: " + meanVal); Console.WriteLine("Span: " + span); Console.WriteLine("Offset: " + offset); ans = (byte)(((meanVal - calValuePitch)-offset)/span); } return ans; }
private static int calValueThrottle = 180; //20% span private static byte ThrottleCalib(PIDManager pid, AdcMessage m) { byte ans; if(m.tas > 12) { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.THROTTLE); double span = pid.GetSpanValue(PIDManager.Ctrl.THROTTLE); int offset = pid.GetOffsetValue(PIDManager.Ctrl.THROTTLE); ans = (byte)(((meanVal - calValueThrottle)-offset)/span); stateThrottle = false; } else if(m.tas < 8) { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.THROTTLE); double span = pid.GetSpanValue(PIDManager.Ctrl.THROTTLE); int offset = pid.GetOffsetValue(PIDManager.Ctrl.THROTTLE); ans = (byte)(((meanVal + calValueThrottle)-offset)/span); stateThrottle = true; } else if(!stateThrottle) { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.THROTTLE); double span = pid.GetSpanValue(PIDManager.Ctrl.THROTTLE); int offset = pid.GetOffsetValue(PIDManager.Ctrl.THROTTLE); ans = (byte)(((meanVal - calValueThrottle)-offset)/span); } else { int meanVal = pid.GetMeanValue(PIDManager.Ctrl.THROTTLE); double span = pid.GetSpanValue(PIDManager.Ctrl.THROTTLE); int offset = pid.GetOffsetValue(PIDManager.Ctrl.THROTTLE); ans = (byte)(((meanVal + calValueThrottle)-offset)/span); } return ans; }