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