internal void setReferenceMeasurement(Measurement referenceMeasurement)
{
referenceJUNK = referenceMeasurement;
referenceMeasurementNED = referenceMeasurement.ConvertFromGeodedicToLocalNED(initialReferenceStartingPositionEFEC, initialReferenceStartingPositionGeodedic);
}
internal void processMeasurement(Measurement newmeasurementgeodedic, System.IO.StreamWriter file, System.IO.StreamWriter file2)
{
//Need to convert new measurement into correct frame.
//lets do this according to the doc. First we go from geodedic to EFEC, then to inertial, then we do some math
//and convert to body.
//so basically get newmeasurement into NED.
//I also have to ensure that the reference measurement gets properly transformed to local NED
/**
* 1. ensure that the new measurement gets transformed to local NED
* 2. ensure that the first new measurement gets set to previous measurement
* 3. ensure that the reference measurement gets transformed to local NED
* 4. Ensure that the takeoff point is transformed to ECEF then used for REF for other conversions to NED
* 5. make sure appropriate rotations are made for local NED to body frame.
* 6. ensure body frame still applies because it said stuff about body only being used for velocity.
*/
Measurement newMeasurementLocalNED = newmeasurementgeodedic.ConvertFromGeodedicToLocalNED(initialReferenceStartingPositionEFEC,
initialReferenceStartingPositionGeodedic);
if (firstMeasurement)
{
firstMeasurement = false;
previousMeasurementNED = newMeasurementLocalNED.Copy();
currentMeasurementNED = newMeasurementLocalNED.Copy();
return;
}
else
{
previousMeasurementNED = currentMeasurementNED.Copy();
currentMeasurementNED = newMeasurementLocalNED.Copy();
}
/* Measurement destinationBody = referenceMeasurementNED.Copy();
destinationBody.Orientation = newMeasurementLocalNED.Orientation;
destinationBody = destinationBody.ConvertFromLocalNEDToBodyFrame();
Measurement newMeasurementBody = newMeasurementLocalNED.Copy();
newMeasurementBody = newMeasurementBody.ConvertFromLocalNEDToBodyFrame();
*/
// Position errorPositionBodyFrame = destinationBody.Position.SubtractPosition(newMeasurementBody.Position);
Position errorPositionNED = referenceMeasurementNED.Position.SubtractPosition(newMeasurementLocalNED.Position);
Measurement temp = newMeasurementLocalNED.Copy();
temp.Position = errorPositionNED;
Position errorPositionBodyFrame = temp.ConvertFromLocalNEDToBodyFrame().Position;
errorPositionBodyFrame.ZAltitudePosition = errorPositionNED.ZAltitudePosition;
//*********
//Calculate integrals
//**********
integralOfXBodyLatitude = integralOfXBodyLatitude + errorPositionBodyFrame.XLatitudePosition * Util.TIMEBETWEENINTERVALS;
//double integralOfYBodyLongitude = calculateIntegral(1);
integralOfYBodyLongitude = integralOfYBodyLongitude + errorPositionBodyFrame.YLongitudePosition * Util.TIMEBETWEENINTERVALS;
// integralOfZBodyAltitude = integralOfZBodyAltitude + errorPositionBodyFrame.ZAltitudePosition;
Measurement currentmeasurementBodyFrame = currentMeasurementNED.ConvertFromLocalNEDToBodyFrame();
Measurement previousmeasurementBodyFrame = previousMeasurementNED.ConvertFromLocalNEDToBodyFrame();
//**********
//Calculate Velocity
//**********
// double xVelocityBodyFrame = (currentmeasurementBodyFrame.Position.XLatitudePosition - previousmeasurementBodyFrame.Position.XLatitudePosition) / Util.TIMEBETWEENINTERVALS;
//double yVelocityBodyFrame = (currentmeasurementBodyFrame.Position.YLongitudePosition - previousmeasurementBodyFrame.Position.YLongitudePosition) / Util.TIMEBETWEENINTERVALS;
double xVelocityBodyFrame = currentmeasurementBodyFrame.Velocity.XVelocityBodyFrame;
double yVelocityBodyFrame = currentmeasurementBodyFrame.Velocity.YVelocityBodyFrame;
//Note: i'm not using body frame z velocity because the error isn't in body frame. I'm rationalizing this because for z we are not
//producing an angle.
//double zVelocityBodyFrame = (currentmeasurementBodyFrame.Position.ZAltitudePosition - previousmeasurementBodyFrame.Position.ZAltitudePosition) / Util.TIMEBETWEENINTERVALS;
// double zVelocityBodyFrame = (currentMeasurementNED.Position.ZAltitudePosition - previousMeasurementNED.Position.ZAltitudePosition) / Util.TIMEBETWEENINTERVALS;
double zVelocityBodyFrame = currentmeasurementBodyFrame.Velocity.ZVelocityBodyFrame;
//Body and ned comparison works here because the velocity for the reference was already in body frame. (m/s)
double velocityErrorX = xVelocityBodyFrame - referenceMeasurementNED.Velocity.XVelocityBodyFrame;
double velocityErrorY = yVelocityBodyFrame - referenceMeasurementNED.Velocity.YVelocityBodyFrame;
double velocityErrorZ = zVelocityBodyFrame - referenceMeasurementNED.Velocity.ZVelocityBodyFrame;
//***************
//Calculate weighted values
//**************
//Calculate weighted integrals
double weightedIntegralOfXBodyLatitude = integralOfXBodyLatitude * Util.K_ETA_X;
double weightedIntegralOfYBodyLongitude = integralOfYBodyLongitude * Util.K_ETA_Y;
// double weightedIntegralOfZBodyAltitude = integralOfZBodyAltitude * Util.K_ETA_Z;
//Calculate weighted proportional values
double weightedProportionalErrorX = errorPositionBodyFrame.XLatitudePosition * Util.K_X;
double weightedProportionalErrorY = errorPositionBodyFrame.YLongitudePosition * Util.K_Y;
double weightedProportionalErrorZ = errorPositionBodyFrame.ZAltitudePosition * Util.K_Z;
double weightedDerivativeVelocityX = velocityErrorX * Util.K_U;
double weightedDerivativeVelocityY = velocityErrorY * Util.K_V;
double weightedDerivativeVelocityZ = velocityErrorZ * Util.K_W;
/*
weightedDerivativeVelocityX = 0;
weightedDerivativeVelocityY = 0;
weightedDerivativeVelocityZ = 0;
*/
/* file2.WriteLine("weightedIntegralOfXBodyLatitude," + weightedIntegralOfXBodyLatitude + ",weightedIntegralOfYBodyLongitude," + weightedIntegralOfYBodyLongitude +
",weightedIntegralOfZBodyAltitude," + weightedIntegralOfZBodyAltitude + ",weightedProportionalErrorX," +weightedProportionalErrorX+
",weightedProportionalErrorY," + weightedProportionalErrorY + ",weightedProportionalErrorZ," + weightedProportionalErrorZ+
",weightedDerivativeVelocityX," + weightedDerivativeVelocityX + ",weightedDerivativeVelocityY,"+weightedDerivativeVelocityY+",weightedDerivativeVelocityZ," +
weightedDerivativeVelocityZ
+ ", phi_roll: " + newMeasurementLocalNED.Orientation.Phi_rollDegrees + ", Sai_yaw: " + newMeasurementLocalNED.Orientation.Sai_yawDegrees + ", theta_pitch" + newMeasurementLocalNED.Orientation.Theta_pitchDegrees
); */
//HACK: CRAP! X CONTROLS THETA WHICH CONTROLS LONGITUDE, NOT LATITUDE.
Console.Write(weightedIntegralOfXBodyLatitude + ", " + weightedProportionalErrorX + ", " + weightedDerivativeVelocityX + "\r");
//Calculate Proportional, Integral, Derivative.
double pidX_thetaDes = weightedIntegralOfXBodyLatitude + weightedProportionalErrorX + weightedDerivativeVelocityX;
double pidY_phiDes = weightedIntegralOfYBodyLongitude + weightedProportionalErrorY + weightedDerivativeVelocityY;
double integralOfZBodyAltitudeTemp = integralOfZBodyAltitude + errorPositionBodyFrame.ZAltitudePosition * Util.TIMEBETWEENINTERVALS;
double weightedIntegralOfZBodyAltitudeTemp = integralOfZBodyAltitudeTemp * Util.K_ETA_Z;
double pidZTemp = -1 * weightedIntegralOfZBodyAltitudeTemp - weightedProportionalErrorZ - weightedDerivativeVelocityZ;
//pidZTemp = -1 * pidZTemp / 10.0;
pidZTemp = -1 * pidZTemp;
double pidZ = 0.0;
double weightedIntegralOfZBodyAltitude = 0.0;
if (pidZTemp > Util.UCOLMAXCONTROLVALUE || pidZTemp < Util.UCOLMINCONTROLVALUE)
{
//integralOfZBodyAltitude = 0;
if (pidZTemp > Util.UCOLMAXCONTROLVALUE)
{
if (integralOfZBodyAltitude > 0)
{
integralOfZBodyAltitude = integralOfZBodyAltitude - Util.Ti_Z * (pidZTemp - Util.UCOLMAXCONTROLVALUE);
}
}
if (pidZTemp < Util.UCOLMINCONTROLVALUE)
{
if (integralOfZBodyAltitude < 0)
{
integralOfZBodyAltitude = integralOfZBodyAltitude - Util.Ti_Z * (pidZTemp - Util.UCOLMINCONTROLVALUE);
}
}
//integrator windup preventor
//integralOfZBodyAltitude = integralOfZBodyAltitude + errorPositionBodyFrame.ZAltitudePosition;
weightedIntegralOfZBodyAltitude = integralOfZBodyAltitude * Util.K_ETA_Z;
pidZ = -1 * weightedIntegralOfZBodyAltitude - weightedProportionalErrorZ - weightedDerivativeVelocityZ;
//pidZ = -1 * pidZ / 10.0;
pidZ = -1 * pidZ;
}
else
{
//IMPORTANT NOTE: i'M NOT multiplying by time interval because the performance is much better without doing that.
//integralOfZBodyAltitude = integralOfZBodyAltitude + errorPositionBodyFrame.ZAltitudePosition * Util.TIMEBETWEENINTERVALS;
integralOfZBodyAltitude = integralOfZBodyAltitude + errorPositionBodyFrame.ZAltitudePosition;
weightedIntegralOfZBodyAltitude = integralOfZBodyAltitude * Util.K_ETA_Z;
pidZ = -1 * weightedIntegralOfZBodyAltitude - weightedProportionalErrorZ - weightedDerivativeVelocityZ;
//pidZ = -1 * pidZ / 10.0;
pidZ = -1 * pidZ;
}
/*if (pidX_thetaDes > 16) pidX_thetaDes = 16;
if (pidX_thetaDes < -16) pidX_thetaDes = -16;*/
if (pidX_thetaDes > 13) pidX_thetaDes = 13;
if (pidX_thetaDes < -13) pidX_thetaDes = -13;
/*if (pidX_thetaDes > 6) pidX_thetaDes = 6;
if (pidX_thetaDes < -2) pidX_thetaDes = -2;*
/*if (pidY_phiDes > 7) pidY_phiDes = 7;
if (pidY_phiDes < -7) pidY_phiDes = -7;*/
if (pidY_phiDes > 10) pidY_phiDes = 10;
if (pidY_phiDes < -10) pidY_phiDes = -10;
/* if (pidY_phiDes > 3) pidY_phiDes = 3;
if (pidY_phiDes < -3) pidY_phiDes = -3;*/
ulon_MainRotorLongitudinalCyclicControlCommand_Pitch = -1 * ((Util.K_THETA * (newMeasurementLocalNED.Orientation.Theta_pitchDegrees - pidX_thetaDes)) + newMeasurementLocalNED.Velocity.PitchVelocityDeg * Util.K_PitchVelocity);
ulat_MainRotorLateralCyclicControlCommand_Roll = -1 * ((Util.K_PHI * (newMeasurementLocalNED.Orientation.Phi_rollDegrees + pidY_phiDes)) + (newMeasurementLocalNED.Velocity.RollVelocityDeg * Util.K_RollVelocity));
ucol_MainRotorCollective_BladeAngle = pidZ;
file.WriteLine("xlat: ," + newMeasurementLocalNED.Position.XLatitudePosition + ", ylong: ," + newMeasurementLocalNED.Position.YLongitudePosition + ", zalt: ," +
newMeasurementLocalNED.Position.ZAltitudePosition + ", geolat: ," + newmeasurementgeodedic.Position.XLatitudePosition + ", geolong: ," + newmeasurementgeodedic.Position.YLongitudePosition + ", geoalt: ," + newmeasurementgeodedic.Position.ZAltitudePosition
+ ", Errorx: ," + errorPositionNED.XLatitudePosition + ", Errory: ," + errorPositionNED.YLongitudePosition + ", Errorz ," + errorPositionNED.ZAltitudePosition + ", errorbodx: ," + errorPositionBodyFrame.XLatitudePosition
+ ", errorbody: ," + errorPositionBodyFrame.YLongitudePosition
+ ", errorbodz: ," + errorPositionBodyFrame.ZAltitudePosition
+ ", pidxthetaDes: ," + pidX_thetaDes
+ ", pidY_phiDes: ," + pidY_phiDes
+ ", pixZ: ," + pidZ
+ ", phi_roll: ," + newMeasurementLocalNED.Orientation.Phi_rollDegrees + ", Sai_yaw: ," + newMeasurementLocalNED.Orientation.Sai_yawDegrees + ", theta_pitch," + newMeasurementLocalNED.Orientation.Theta_pitchDegrees
+ ", ulon_pitch: ," + ulon_MainRotorLongitudinalCyclicControlCommand_Pitch
+ ", ulat_roll: ," + ulat_MainRotorLateralCyclicControlCommand_Roll
+ ", collective: ," + ucol_MainRotorCollective_BladeAngle
+ ", errorbodyx, " + errorPositionBodyFrame.XLatitudePosition
+ ", errorbodyy," + errorPositionBodyFrame.YLongitudePosition
+ ", errorbodyz," + errorPositionBodyFrame.ZAltitudePosition
+ "weightedIntegralOfXBodyLatitude," + weightedIntegralOfXBodyLatitude + ",weightedIntegralOfYBodyLongitude," + weightedIntegralOfYBodyLongitude +
",weightedIntegralOfZBodyAltitude," + weightedIntegralOfZBodyAltitude + ",weightedProportionalErrorX," + weightedProportionalErrorX +
",weightedProportionalErrorY," + weightedProportionalErrorY + ",weightedProportionalErrorZ," + weightedProportionalErrorZ +
",weightedDerivativeVelocityX," + weightedDerivativeVelocityX + ",weightedDerivativeVelocityY," + weightedDerivativeVelocityY + ",weightedDerivativeVelocityZ," +
weightedDerivativeVelocityZ
+ ", phi_roll: " + newMeasurementLocalNED.Orientation.Phi_rollDegrees + ", Sai_yaw: " + newMeasurementLocalNED.Orientation.Sai_yawDegrees + ", theta_pitch" + newMeasurementLocalNED.Orientation.Theta_pitchDegrees
);
//yawErrors.Enqueue(weightedProportionalErrorX);
// yawErrors.Enqueue(weightedProportionalErrorZ);
// yawErrors.Enqueue(errorPositionNED.YLongitudePosition);
yawErrors.Enqueue(errorPositionNED.XLatitudePosition);
// derivativeYaw.Enqueue(weightedDerivativeVelocityY);
// derivativeYaw.Enqueue(weightedDerivativeVelocityX);
// derivativeYaw.Enqueue(weightedDerivativeVelocityZ);
derivativeYaw.Enqueue(errorPositionNED.YLongitudePosition);
// integralYaw.Enqueue(weightedIntegralOfYBodyLongitude);
// integralYaw.Enqueue(weightedIntegralOfYBodyLongitude);
// integralYaw.Enqueue(weightedIntegralOfXBodyLatitude);
//integralYaw.Enqueue(errorPositionNED.ZAltitudePosition);
integralYaw.Enqueue(errorPositionNED.ZAltitudePosition);
// Ped.Enqueue(ulat_MainRotorLateralCyclicControlCommand_Roll);
Ped.Enqueue(ucol_MainRotorCollective_BladeAngle);
if (yawErrors.Count > 400)
{
yawErrors.Dequeue();
derivativeYaw.Dequeue();
integralYaw.Dequeue();
Ped.Dequeue();
}
List<double> myList = new List<double>();
myList.AddRange(yawErrors);
Form1.plotLineGraphChart1(myList);
myList = new List<double>();
myList.AddRange(derivativeYaw);
Form1.plotLineGraph(myList, Form1.chart2);
myList = new List<double>();
myList.AddRange(integralYaw);
Form1.plotLineGraph(myList, Form1.chart3);
myList = new List<double>();
myList.AddRange(Ped);
Form1.plotLineGraph(myList, Form1.chart4);
/**
* yaw processing
*/
double yawError = newMeasurementLocalNED.Orientation.Sai_yawDegrees - referenceMeasurementNED.Orientation.Sai_yawDegrees;
if (yawError > 180)
{
yawError = yawError - 360;
}
else if (yawError < -180)
{
yawError = yawError + 360;
}
/*
if (newMeasurementLocalNED.Orientation.Sai_yawDegrees < referenceMeasurementNED.Orientation.Sai_yawDegrees &&
previousMeasurementNED.Orientation.Sai_yawDegrees > referenceMeasurementNED.Orientation.Sai_yawDegrees)
{
//Boolean going clockwise = false
isGoingClockwise = false;
}
else if (newMeasurementLocalNED.Orientation.Sai_yawDegrees > referenceMeasurementNED.Orientation.Sai_yawDegrees &&
previousMeasurementNED.Orientation.Sai_yawDegrees < referenceMeasurementNED.Orientation.Sai_yawDegrees)
{
isGoingClockwise = true;
}
double yawError = 0.0;
if (isGoingClockwise)
{
yawError = newMeasurementLocalNED.Orientation.Sai_yawDegrees - referenceMeasurementNED.Orientation.Sai_yawDegrees;
}
else
{
yawError = newMeasurementLocalNED.Orientation.Sai_yawDegrees - referenceMeasurementNED.Orientation.Sai_yawDegrees;
}
*/
/*if (yawErrors.Count > 40)
{
yawErrors.Dequeue();
}*/
/**
* HORRIBLE WAY OF DOING INTEGRAL ANTI-WINDUP
*/
integralOfYaw = integralOfYaw + yawError * Util.TIMEBETWEENINTERVALS;
double weightedIntegralOfYawError = integralOfYaw * Util.K_ETA_SAI;
double weightedProportionalOfYawError = yawError * Util.K_SAI;
//double derivativeOfYaw = (newMeasurementLocalNED.Orientation.Sai_yawDegrees - previousMeasurementNED.Orientation.Sai_yawDegrees) / Util.TIMEBETWEENINTERVALS;
double derivativeOfYaw = newMeasurementLocalNED.Velocity.YawVelocity;
//double yawDerivativeError = referenceMeasurementNED.Velocity.YawVelocity - derivativeOfYaw;
double yawDerivativeError = derivativeOfYaw - referenceMeasurementNED.Velocity.YawVelocity; //it goes out of control the other way.
//double weightedYawDerivativeError = yawDerivativeError * Util.K_R;
//double weightedYawDerivativeError = referenceMeasurementNED.Velocity.YawVelocity * Util.K_R;
//double weightedYawDerivativeError = currentMeasurementNED.Velocity.YawVelocity * Util.K_R;
double weightedYawDerivativeError = yawDerivativeError * Util.K_R;
//control law from: http://books.google.com/books?id=Xqzv_wXqC5EC&pg=PA516&lpg=PA516&dq=pid+controller+tail+rotor&source=bl&ots=PxlTw8tb_Y&sig=u0iBG5m6hV4VeY1yM9ApTPHtO2Q&hl=en#v=onepage&q=pid%20controller%20tail%20rotor&f=false
//******** note added a * -1 here.
uped_TailRotorPedal_BladeAngleOnTail = (-1 * weightedIntegralOfYawError - weightedProportionalOfYawError - weightedYawDerivativeError) * -1;
if (uped_TailRotorPedal_BladeAngleOnTail > Util.UPEDMAXCONTROLVALUE ||
uped_TailRotorPedal_BladeAngleOnTail < Util.UPEDMINCONTROLVALUE)
{
if (uped_TailRotorPedal_BladeAngleOnTail > Util.UPEDMAXCONTROLVALUE)
{
if (integralOfYaw > 0)
{
integralOfYaw = integralOfYaw - Util.Ti_SAI * (uped_TailRotorPedal_BladeAngleOnTail - Util.UPEDMAXCONTROLVALUE);
}
}
if (integralOfYaw < Util.UPEDMINCONTROLVALUE)
{
if (integralOfYaw < 0)
{
integralOfYaw = integralOfYaw - Util.Ti_SAI * (uped_TailRotorPedal_BladeAngleOnTail - Util.UPEDMINCONTROLVALUE);
}
}
// integralOfYaw = integralOfYaw - yawError * Util.TIMEBETWEENINTERVALS;
// integralOfYaw = 0;
weightedIntegralOfYawError = integralOfYaw * Util.K_ETA_SAI;
uped_TailRotorPedal_BladeAngleOnTail = (-1 * weightedIntegralOfYawError - weightedProportionalOfYawError - weightedYawDerivativeError) * -1;
}
/**
* END HORRIBLE WAY OF INTIGRAL ANTIWINDUP
*/
// yawErrors.Enqueue(weightedProportionalOfYawError);
// yawErrors.Enqueue(errorPositionBodyFrame.ZAltitudePosition);
/* yawErrors.Enqueue(yawError);
derivativeYaw.Enqueue(weightedYawDerivativeError);
integralYaw.Enqueue(weightedIntegralOfYawError);
Ped.Enqueue(uped_TailRotorPedal_BladeAngleOnTail);
if (yawErrors.Count > 200)
{
yawErrors.Dequeue();
derivativeYaw.Dequeue();
integralYaw.Dequeue();
Ped.Dequeue();
}
List<double> myList = new List<double>();
myList.AddRange(yawErrors);
Form1.plotLineGraphChart1(myList);
myList = new List<double>();
myList.AddRange(derivativeYaw);
Form1.plotLineGraph(myList, Form1.chart2);
myList = new List<double>();
myList.AddRange(integralYaw);
Form1.plotLineGraph(myList, Form1.chart3);
myList = new List<double>();
myList.AddRange(Ped);
Form1.plotLineGraph(myList, Form1.chart4);
*/
/*
yawErrors.Enqueue(yawError);
derivativeYaw.Enqueue(derivativeOfYaw);
integralYaw.Enqueue(integralOfYaw);
Ped.Enqueue(uped_TailRotorPedal_BladeAngleOnTail);
if (yawErrors.Count > 400)
{
yawErrors.Dequeue();
derivativeYaw.Dequeue();
integralYaw.Dequeue();
Ped.Dequeue();
}
List<double> myList = new List<double>();
myList.AddRange(yawErrors);
Form1.plotLineGraphChart1(myList);
myList = new List<double>();
myList.AddRange(derivativeYaw);
Form1.plotLineGraph(myList, Form1.chart2);
myList = new List<double>();
myList.AddRange(integralYaw);
Form1.plotLineGraph(myList, Form1.chart3);
myList = new List<double>();
myList.AddRange(Ped);
Form1.plotLineGraph(myList, Form1.chart4);
*/
if (refreshcounter == 5)
{
Application.DoEvents();
refreshcounter = 0;
}
else
{
refreshcounter++;
}
/* file.WriteLine(newmeasurement.Orientation.Sai_yawDegrees + ", " + referenceMeasurementNED.Orientation.Sai_yawDegrees + ", " +
yawError + ", " + weightedProportionalOfYawError);*/
// Console.Write(weightedIntegralOfZBodyAltitude + ", " + weightedProportionalErrorZ + ", " + weightedDerivativeVelocityZ + "\r");
}
internal void processMeasurement(Measurement newmeasurementgeodedic, System.IO.StreamWriter file)
{
//Need to convert new measurement into correct frame.
//lets do this according to the doc. First we go from geodedic to EFEC, then to inertial, then we do some math
//and convert to body.
//so basically get newmeasurement into NED.
//I also have to ensure that the reference measurement gets properly transformed to local NED
/**
* 1. ensure that the new measurement gets transformed to local NED
* 2. ensure that the first new measurement gets set to previous measurement
* 3. ensure that the reference measurement gets transformed to local NED
* 4. Ensure that the takeoff point is transformed to ECEF then used for REF for other conversions to NED
* 5. make sure appropriate rotations are made for local NED to body frame.
* 6. ensure body frame still applies because it said stuff about body only being used for velocity.
*/
Measurement newMeasurementLocalNED = newmeasurementgeodedic.ConvertFromGeodedicToLocalNED(initialReferenceStartingPositionEFEC,
initialReferenceStartingPositionGeodedic);
if (firstMeasurement)
{
firstMeasurement = false;
previousMeasurementNED = newMeasurementLocalNED.Copy();
currentMeasurementNED = newMeasurementLocalNED.Copy();
return;
}
else
{
previousMeasurementNED = currentMeasurementNED.Copy();
currentMeasurementNED = newMeasurementLocalNED.Copy();
}
Position errorPositionNED = referenceMeasurementNED.Position.SubtractPosition(newMeasurementLocalNED.Position);
Measurement temp = newMeasurementLocalNED.Copy();
temp.Position = errorPositionNED;
Position errorPositionBodyFrame = temp.ConvertFromLocalNEDToBodyFrame().Position;
//*********
//Calculate integrals
//**********
integralOfXBodyLatitude = integralOfXBodyLatitude + errorPositionBodyFrame.XLatitudePosition;
//double integralOfYBodyLongitude = calculateIntegral(1);
integralOfYBodyLongitude = integralOfYBodyLongitude + errorPositionBodyFrame.YLongitudePosition;
integralOfZBodyAltitude = integralOfZBodyAltitude + errorPositionBodyFrame.ZAltitudePosition;
Measurement currentmeasurementBodyFrame = currentMeasurementNED.ConvertFromLocalNEDToBodyFrame();
Measurement previousmeasurementBodyFrame = previousMeasurementNED.ConvertFromLocalNEDToBodyFrame();
//**********
//Calculate Velocity
//**********
double xVelocityBodyFrame = (currentmeasurementBodyFrame.Position.XLatitudePosition - previousmeasurementBodyFrame.Position.XLatitudePosition) / Util.TIMEBETWEENINTERVALS;
double yVelocityBodyFrame = (currentmeasurementBodyFrame.Position.YLongitudePosition - previousmeasurementBodyFrame.Position.YLongitudePosition) / Util.TIMEBETWEENINTERVALS;
double zVelocityBodyFrame = (currentmeasurementBodyFrame.Position.ZAltitudePosition - previousmeasurementBodyFrame.Position.ZAltitudePosition) / Util.TIMEBETWEENINTERVALS;
//Body and ned comparison works here because the velocity for the reference was already in body frame. (m/s)
double velocityErrorX = xVelocityBodyFrame - referenceMeasurementNED.Velocity.XVelocityBodyFrame;
double velocityErrorY = yVelocityBodyFrame - referenceMeasurementNED.Velocity.YVelocityBodyFrame;
double velocityErrorZ = zVelocityBodyFrame - referenceMeasurementNED.Velocity.ZVelocityBodyFrame;
//***************
//Calculate weighted values
//**************
//Calculate weighted integrals
double weightedIntegralOfXBodyLatitude = integralOfXBodyLatitude * Util.K_ETA_X;
double weightedIntegralOfYBodyLongitude = integralOfYBodyLongitude * Util.K_ETA_Y;
double weightedIntegralOfZBodyAltitude = integralOfZBodyAltitude * Util.K_ETA_Z;
//Calculate weighted proportional values
double weightedProportionalErrorX = errorPositionBodyFrame.XLatitudePosition * Util.K_X;
double weightedProportionalErrorY = errorPositionBodyFrame.YLongitudePosition * Util.K_Y;
double weightedProportionalErrorZ = errorPositionBodyFrame.ZAltitudePosition * Util.K_Z;
double weightedDerivativeVelocityX = velocityErrorX * Util.K_U;
double weightedDerivativeVelocityY = velocityErrorY * Util.K_V;
double weightedDerivativeVelocityZ = velocityErrorZ * Util.K_W;
Console.Write(weightedIntegralOfXBodyLatitude + ", " + weightedProportionalErrorX + ", " + weightedDerivativeVelocityX + "\r");
//Calculate Proportional, Integral, Derivative.
double pidX_thetaDes = weightedIntegralOfXBodyLatitude + weightedProportionalErrorX + weightedDerivativeVelocityX;
double pidY_phiDes = weightedIntegralOfYBodyLongitude + weightedProportionalErrorY + weightedDerivativeVelocityY;
double pidZ = -1 * weightedIntegralOfZBodyAltitude - weightedProportionalErrorZ - weightedDerivativeVelocityZ;
ulon_MainRotorLongitudinalCyclicControlCommand_Pitch = -1 * Util.K_THETA * (newMeasurementLocalNED.Orientation.Theta_pitchDegrees - pidX_thetaDes);
ulat_MainRotorLateralCyclicControlCommand_Roll = -1 * Util.K_PHI * (newMeasurementLocalNED.Orientation.Phi_rollDegrees + pidY_phiDes);
ucol_MainRotorCollective_BladeAngle = pidZ;
/**
* yaw processing
*/
/*double yawError = newMeasurementLocalNED.Orientation.Sai_yawDegrees - referenceMeasurementNED.Orientation.Sai_yawDegrees;
if (yawError > 180)
{
yawError = yawError - 360;
}
else if (yawError < -180)
{
yawError = yawError + 360;
}*/
//double yawError = newMeasurementLocalNED.Orientation.Sai_yawDegrees - referenceMeasurementNED.Orientation.Sai_yawDegrees;
//if the current measurement is < ref measure and previous measurement > ref measurement
// then you crossed the threshold going the counter clockwise direction
// the error should be 0-360 positive
//else if the current measurement is > ref measurement, and previous measurement < ref measurement
// then you crossed the threshold going the clockwise direction.
// the error should be 0-360 negative.
if (newMeasurementLocalNED.Orientation.Sai_yawDegrees < referenceMeasurementNED.Orientation.Sai_yawDegrees &&
previousMeasurementNED.Orientation.Sai_yawDegrees > referenceMeasurementNED.Orientation.Sai_yawDegrees)
{
//Boolean going clockwise = false
isGoingClockwise = false;
}else if (newMeasurementLocalNED.Orientation.Sai_yawDegrees > referenceMeasurementNED.Orientation.Sai_yawDegrees &&
previousMeasurementNED.Orientation.Sai_yawDegrees < referenceMeasurementNED.Orientation.Sai_yawDegrees)
{
isGoingClockwise = true;
}
double yawError = 0.0;
if (isGoingClockwise)
{
yawError = newMeasurementLocalNED.Orientation.Sai_yawDegrees - referenceMeasurementNED.Orientation.Sai_yawDegrees;
}
else
{
yawError = newMeasurementLocalNED.Orientation.Sai_yawDegrees - referenceMeasurementNED.Orientation.Sai_yawDegrees;
}
yawErrors.Enqueue(yawError);
if (yawErrors.Count > 40)
{
yawErrors.Dequeue();
}
integralOfYaw = integralOfYaw + yawError;
double weightedIntegralOfYawError = integralOfYaw * Util.K_ETA_SAI;
double weightedProportionalOfYawError = yawError * Util.K_SAI;
//double derivativeOfYaw = (newMeasurementLocalNED.Orientation.Sai_yawDegrees - previousMeasurementNED.Orientation.Sai_yawDegrees) / Util.TIMEBETWEENINTERVALS;
double derivativeOfYaw = newMeasurementLocalNED.Velocity.YawVelocity;
double yawDerivativeError = referenceMeasurementNED.Velocity.YawVelocity - derivativeOfYaw;
//double weightedYawDerivativeError = yawDerivativeError * Util.K_R;
double weightedYawDerivativeError = referenceMeasurementNED.Velocity.YawVelocity * Util.K_R;
//control law from: http://books.google.com/books?id=Xqzv_wXqC5EC&pg=PA516&lpg=PA516&dq=pid+controller+tail+rotor&source=bl&ots=PxlTw8tb_Y&sig=u0iBG5m6hV4VeY1yM9ApTPHtO2Q&hl=en#v=onepage&q=pid%20controller%20tail%20rotor&f=false
uped_TailRotorPedal_BladeAngleOnTail = -1 * weightedIntegralOfYawError - weightedProportionalOfYawError - weightedYawDerivativeError;
/* file.WriteLine(newmeasurement.Orientation.Sai_yawDegrees + ", " + referenceMeasurementNED.Orientation.Sai_yawDegrees + ", " +
yawError + ", " + weightedProportionalOfYawError);*/
// Console.Write(weightedIntegralOfZBodyAltitude + ", " + weightedProportionalErrorZ + ", " + weightedDerivativeVelocityZ + "\r");
}