public void GetCurrentCurveLine(vec3 pivot, vec3 steer)
{
//determine closest point
double minDistance = 9999999;
int ptCount = refList.Count;
int ptCnt = ptCount - 1;
if (ptCount < 5)
{
return;
}
boxA.easting = pivot.easting - (Math.Sin(aveLineHeading + glm.PIBy2) * 2000);
boxA.northing = pivot.northing - (Math.Cos(aveLineHeading + glm.PIBy2) * 2000);
boxB.easting = pivot.easting + (Math.Sin(aveLineHeading + glm.PIBy2) * 2000);
boxB.northing = pivot.northing + (Math.Cos(aveLineHeading + glm.PIBy2) * 2000);
boxC.easting = boxB.easting + (Math.Sin(aveLineHeading) * 1.0);
boxC.northing = boxB.northing + (Math.Cos(aveLineHeading) * 1.0);
boxD.easting = boxA.easting + (Math.Sin(aveLineHeading) * 1.0);
boxD.northing = boxA.northing + (Math.Cos(aveLineHeading) * 1.0);
boxA.easting -= (Math.Sin(aveLineHeading) * 1.0);
boxA.northing -= (Math.Cos(aveLineHeading) * 1.0);
boxB.easting -= (Math.Sin(aveLineHeading) * 1.0);
boxB.northing -= (Math.Cos(aveLineHeading) * 1.0);
//determine if point are in frustum box
for (int s = 0; s < ptCnt; s++)
{
if ((((boxB.easting - boxA.easting) * (refList[s].northing - boxA.northing))
- ((boxB.northing - boxA.northing) * (refList[s].easting - boxA.easting))) < 0)
{
continue;
}
if ((((boxD.easting - boxC.easting) * (refList[s].northing - boxC.northing))
- ((boxD.northing - boxC.northing) * (refList[s].easting - boxC.easting))) < 0)
{
continue;
}
closestRefIndex = s;
break;
}
double dist = ((pivot.easting - refList[closestRefIndex].easting) * (pivot.easting - refList[closestRefIndex].easting))
+ ((pivot.northing - refList[closestRefIndex].northing) * (pivot.northing - refList[closestRefIndex].northing));
minDistance = Math.Sqrt(dist);
//grab the heading at the closest point
refHeading = refList[closestRefIndex].heading;
//which side of the patch are we on is next
//calculate endpoints of reference line based on closest point
refPoint1.easting = refList[closestRefIndex].easting - (Math.Sin(refHeading) * 50.0);
refPoint1.northing = refList[closestRefIndex].northing - (Math.Cos(refHeading) * 50.0);
refPoint2.easting = refList[closestRefIndex].easting + (Math.Sin(refHeading) * 50.0);
refPoint2.northing = refList[closestRefIndex].northing + (Math.Cos(refHeading) * 50.0);
//x2-x1
double dx = refPoint2.easting - refPoint1.easting;
//z2-z1
double dz = refPoint2.northing - refPoint1.northing;
//how far are we away from the reference line at 90 degrees - 2D cross product and distance
double distanceFromRefLine = ((dz * pivot.easting) - (dx * pivot.northing) + (refPoint2.easting
* refPoint1.northing) - (refPoint2.northing * refPoint1.easting));
// / Math.Sqrt((dz * dz) + (dx * dx));
//are we going same direction as stripList was created?
isSameWay = Math.PI - Math.Abs(Math.Abs(pivot.heading - refHeading) - Math.PI) < glm.PIBy2;
deltaOfRefAndAveHeadings = Math.PI - Math.Abs(Math.Abs(aveLineHeading - refHeading) - Math.PI);
deltaOfRefAndAveHeadings = Math.Cos(deltaOfRefAndAveHeadings);
//add or subtract pi by 2 depending on which side of ref line
double piSide;
//sign of distance determines which side of line we are on
if (distanceFromRefLine > 0)
{
piSide = glm.PIBy2;
}
else
{
piSide = -glm.PIBy2;
}
//move the ABLine over based on the overlap amount set in vehicle
double widthMinusOverlap = mf.vehicle.toolWidth - mf.vehicle.toolOverlap;
howManyPathsAway = Math.Round(minDistance / widthMinusOverlap, 0, MidpointRounding.AwayFromZero);
//build the current line
curList?.Clear();
for (int i = 0; i < ptCount; i++)
{
var point = new vec3(
refList[i].easting + (Math.Sin(piSide + aveLineHeading) * widthMinusOverlap * howManyPathsAway),
refList[i].northing + (Math.Cos(piSide + aveLineHeading) * widthMinusOverlap * howManyPathsAway),
refList[i].heading);
curList.Add(point);
}
double minDistA = 1000000, minDistB = 1000000;
ptCount = curList.Count;
if (ptCount > 0)
{
if (mf.isStanleyUsed)
{
//find the closest 2 points to current fix
for (int t = 0; t < ptCount; t++)
{
dist = ((steer.easting - curList[t].easting) * (steer.easting - curList[t].easting))
+ ((steer.northing - curList[t].northing) * (steer.northing - curList[t].northing));
if (dist < minDistA)
{
minDistB = minDistA;
B = A;
minDistA = dist;
A = t;
}
else if (dist < minDistB)
{
minDistB = dist;
B = t;
}
}
//just need to make sure the points continue ascending or heading switches all over the place
if (A > B)
{
C = A; A = B; B = C;
}
currentLocationIndex = A;
//get the distance from currently active AB line
dx = curList[B].easting - curList[A].easting;
dz = curList[B].northing - curList[A].northing;
if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dz) < Double.Epsilon)
{
return;
}
//abHeading = Math.Atan2(dz, dx);
double abHeading = curList[A].heading;
//how far from current AB Line is fix
distanceFromCurrentLine = ((dz * steer.easting) - (dx * steer.northing) + (curList[B].easting
* curList[A].northing) - (curList[B].northing * curList[A].easting))
/ Math.Sqrt((dz * dz) + (dx * dx));
//are we on the right side or not
isOnRightSideCurrentLine = distanceFromCurrentLine > 0;
//absolute the distance
distanceFromCurrentLine = Math.Abs(distanceFromCurrentLine);
//Subtract the two headings, if > 1.57 its going the opposite heading as refAB
double abFixHeadingDelta = (Math.Abs(mf.fixHeading - abHeading));
if (abFixHeadingDelta >= Math.PI)
{
abFixHeadingDelta = Math.Abs(abFixHeadingDelta - glm.twoPI);
}
isABSameAsVehicleHeading = abFixHeadingDelta < glm.PIBy2;
// calc point on ABLine closest to current position
double U = (((steer.easting - curList[A].easting) * dx)
+ ((steer.northing - curList[A].northing) * dz))
/ ((dx * dx) + (dz * dz));
rEastCu = curList[A].easting + (U * dx);
rNorthCu = curList[A].northing + (U * dz);
//distance is negative if on left, positive if on right
if (isABSameAsVehicleHeading)
{
if (!isOnRightSideCurrentLine)
{
distanceFromCurrentLine *= -1.0;
}
abFixHeadingDelta = (steer.heading - abHeading);
}
//opposite way so right is left
else
{
if (isOnRightSideCurrentLine)
{
distanceFromCurrentLine *= -1.0;
}
abFixHeadingDelta = (steer.heading - abHeading + Math.PI);
}
//Fix the circular error
if (abFixHeadingDelta > Math.PI)
{
abFixHeadingDelta -= Math.PI;
}
else if (abFixHeadingDelta < Math.PI)
{
abFixHeadingDelta += Math.PI;
}
if (abFixHeadingDelta > glm.PIBy2)
{
abFixHeadingDelta -= Math.PI;
}
else if (abFixHeadingDelta < -glm.PIBy2)
{
abFixHeadingDelta += Math.PI;
}
abFixHeadingDelta *= mf.vehicle.stanleyHeadingErrorGain;
if (abFixHeadingDelta > 0.74)
{
abFixHeadingDelta = 0.74;
}
if (abFixHeadingDelta < -0.74)
{
abFixHeadingDelta = -0.74;
}
steerAngleCu = Math.Atan((distanceFromCurrentLine * mf.vehicle.stanleyGain) / ((mf.pn.speed * 0.277777) + 1));
if (steerAngleCu > 0.74)
{
steerAngleCu = 0.74;
}
if (steerAngleCu < -0.74)
{
steerAngleCu = -0.74;
}
steerAngleCu = glm.toDegrees((steerAngleCu + abFixHeadingDelta) * -1.0);
if (steerAngleCu < -mf.vehicle.maxSteerAngle)
{
steerAngleCu = -mf.vehicle.maxSteerAngle;
}
if (steerAngleCu > mf.vehicle.maxSteerAngle)
{
steerAngleCu = mf.vehicle.maxSteerAngle;
}
//Convert to millimeters
distanceFromCurrentLine = Math.Round(distanceFromCurrentLine * 1000.0, MidpointRounding.AwayFromZero);
}
else
{
//find the closest 2 points to current fix
for (int t = 0; t < ptCount; t++)
{
dist = ((pivot.easting - curList[t].easting) * (pivot.easting - curList[t].easting))
+ ((pivot.northing - curList[t].northing) * (pivot.northing - curList[t].northing));
if (dist < minDistA)
{
minDistB = minDistA;
B = A;
minDistA = dist;
A = t;
}
else if (dist < minDistB)
{
minDistB = dist;
B = t;
}
}
//just need to make sure the points continue ascending or heading switches all over the place
if (A > B)
{
C = A; A = B; B = C;
}
currentLocationIndex = A;
//get the distance from currently active AB line
dx = curList[B].easting - curList[A].easting;
dz = curList[B].northing - curList[A].northing;
if (Math.Abs(dx) < Double.Epsilon && Math.Abs(dz) < Double.Epsilon)
{
return;
}
//abHeading = Math.Atan2(dz, dx);
double abHeading = curList[A].heading;
//how far from current AB Line is fix
distanceFromCurrentLine = ((dz * pivot.easting) - (dx * pivot.northing) + (curList[B].easting
* curList[A].northing) - (curList[B].northing * curList[A].easting))
/ Math.Sqrt((dz * dz) + (dx * dx));
//are we on the right side or not
isOnRightSideCurrentLine = distanceFromCurrentLine > 0;
//absolute the distance
distanceFromCurrentLine = Math.Abs(distanceFromCurrentLine);
// ** Pure pursuit ** - calc point on ABLine closest to current position
double U = (((pivot.easting - curList[A].easting) * dx)
+ ((pivot.northing - curList[A].northing) * dz))
/ ((dx * dx) + (dz * dz));
rEastCu = curList[A].easting + (U * dx);
rNorthCu = curList[A].northing + (U * dz);
//double minx, maxx, miny, maxy;
//minx = Math.Min(curList[A].northing, curList[B].northing);
//maxx = Math.Max(curList[A].northing, curList[B].northing);
//miny = Math.Min(curList[A].easting, curList[B].easting);
//maxy = Math.Max(curList[A].easting, curList[B].easting);
//isValid = rNorthCu >= minx && rNorthCu <= maxx && (rEastCu >= miny && rEastCu <= maxy);
//if (!isValid)
//{
// //invalid distance so tell AS module
// distanceFromCurrentLine = 32000;
// mf.guidanceLineDistanceOff = 32000;
// return;
//}
//used for accumulating distance to find goal point
double distSoFar;
//update base on autosteer settings and distance from line
double goalPointDistance = mf.vehicle.UpdateGoalPointDistance(distanceFromCurrentLine);
mf.lookaheadActual = goalPointDistance;
// used for calculating the length squared of next segment.
double tempDist = 0.0;
if (!isSameWay)
{
//counting down
isABSameAsVehicleHeading = false;
distSoFar = glm.Distance(curList[A], rEastCu, rNorthCu);
//Is this segment long enough to contain the full lookahead distance?
if (distSoFar > goalPointDistance)
{
//treat current segment like an AB Line
goalPointCu.easting = rEastCu - (Math.Sin(curList[A].heading) * goalPointDistance);
goalPointCu.northing = rNorthCu - (Math.Cos(curList[A].heading) * goalPointDistance);
}
//multiple segments required
else
{
//cycle thru segments and keep adding lengths. check if start and break if so.
while (A > 0)
{
B--; A--;
tempDist = glm.Distance(curList[B], curList[A]);
//will we go too far?
if ((tempDist + distSoFar) > goalPointDistance)
{
break; //tempDist contains the full length of next segment
}
else
{
distSoFar += tempDist;
}
}
double t = (goalPointDistance - distSoFar); // the remainder to yet travel
t /= tempDist;
goalPointCu.easting = (((1 - t) * curList[B].easting) + (t * curList[A].easting));
goalPointCu.northing = (((1 - t) * curList[B].northing) + (t * curList[A].northing));
}
}
else
{
//counting up
isABSameAsVehicleHeading = true;
distSoFar = glm.Distance(curList[B], rEastCu, rNorthCu);
//Is this segment long enough to contain the full lookahead distance?
if (distSoFar > goalPointDistance)
{
//treat current segment like an AB Line
goalPointCu.easting = rEastCu + (Math.Sin(curList[A].heading) * goalPointDistance);
goalPointCu.northing = rNorthCu + (Math.Cos(curList[A].heading) * goalPointDistance);
}
//multiple segments required
else
{
//cycle thru segments and keep adding lengths. check if end and break if so.
// ReSharper disable once LoopVariableIsNeverChangedInsideLoop
while (B < ptCount - 1)
{
B++; A++;
tempDist = glm.Distance(curList[B], curList[A]);
//will we go too far?
if ((tempDist + distSoFar) > goalPointDistance)
{
//A--; B--;
break; //tempDist contains the full length of next segment
}
distSoFar += tempDist;
}
//xt = (((1 - t) * x0 + t * x1)
//yt = ((1 - t) * y0 + t * y1))
double t = (goalPointDistance - distSoFar); // the remainder to yet travel
t /= tempDist;
goalPointCu.easting = (((1 - t) * curList[A].easting) + (t * curList[B].easting));
goalPointCu.northing = (((1 - t) * curList[A].northing) + (t * curList[B].northing));
}
}
//calc "D" the distance from pivot axle to lookahead point
double goalPointDistanceSquared = glm.DistanceSquared(goalPointCu.northing, goalPointCu.easting, pivot.northing, pivot.easting);
//calculate the the delta x in local coordinates and steering angle degrees based on wheelbase
double localHeading = glm.twoPI - mf.fixHeading;
ppRadiusCu = goalPointDistanceSquared / (2 * (((goalPointCu.easting - pivot.easting) * Math.Cos(localHeading)) + ((goalPointCu.northing - pivot.northing) * Math.Sin(localHeading))));
steerAngleCu = glm.toDegrees(Math.Atan(2 * (((goalPointCu.easting - pivot.easting) * Math.Cos(localHeading))
+ ((goalPointCu.northing - pivot.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase / goalPointDistanceSquared));
if (steerAngleCu < -mf.vehicle.maxSteerAngle)
{
steerAngleCu = -mf.vehicle.maxSteerAngle;
}
if (steerAngleCu > mf.vehicle.maxSteerAngle)
{
steerAngleCu = mf.vehicle.maxSteerAngle;
}
if (ppRadiusCu < -500)
{
ppRadiusCu = -500;
}
if (ppRadiusCu > 500)
{
ppRadiusCu = 500;
}
radiusPointCu.easting = pivot.easting + (ppRadiusCu * Math.Cos(localHeading));
radiusPointCu.northing = pivot.northing + (ppRadiusCu * Math.Sin(localHeading));
//angular velocity in rads/sec = 2PI * m/sec * radians/meters
double angVel = glm.twoPI * 0.277777 * mf.pn.speed * (Math.Tan(glm.toRadians(steerAngleCu))) / mf.vehicle.wheelbase;
//clamp the steering angle to not exceed safe angular velocity
if (Math.Abs(angVel) > mf.vehicle.maxAngularVelocity)
{
steerAngleCu = glm.toDegrees(steerAngleCu > 0 ?
(Math.Atan((mf.vehicle.wheelbase * mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777)))
: (Math.Atan((mf.vehicle.wheelbase * -mf.vehicle.maxAngularVelocity) / (glm.twoPI * mf.pn.speed * 0.277777))));
}
//Convert to centimeters
distanceFromCurrentLine = Math.Round(distanceFromCurrentLine * 1000.0, MidpointRounding.AwayFromZero);
//distance is negative if on left, positive if on right
//if you're going the opposite direction left is right and right is left
//double temp;
if (isABSameAsVehicleHeading)
{
//temp = (abHeading);
if (!isOnRightSideCurrentLine)
{
distanceFromCurrentLine *= -1.0;
}
}
//opposite way so right is left
else
{
//temp = (abHeading - Math.PI);
//if (temp < 0) temp = (temp + glm.twoPI);
//temp = glm.toDegrees(temp);
if (isOnRightSideCurrentLine)
{
distanceFromCurrentLine *= -1.0;
}
}
}
mf.guidanceLineDistanceOff = (Int16)distanceFromCurrentLine;
mf.guidanceLineSteerAngle = (Int16)(steerAngleCu * 100);
}
else
{
//invalid distance so tell AS module
distanceFromCurrentLine = 32000;
mf.guidanceLineDistanceOff = 32000;
}
}
private void UpdateFixPosition()
{
startCounter++;
totalFixSteps = fixUpdateHz * 6;
if (!isGPSPositionInitialized) { InitializeFirstFewGPSPositions(); return; }
#region Antenna Offset
if (vehicle.antennaOffset != 0)
{
if (vehicle.antennaOffset < 0)
{
offset -= 0.01;
if (offset < vehicle.antennaOffset) offset = vehicle.antennaOffset;
pn.fix.easting = (Math.Cos(-fixHeading) * offset) + pn.fix.easting;
pn.fix.northing = (Math.Sin(-fixHeading) * offset) + pn.fix.northing;
}
else
{
offset += 0.01;
if (offset > vehicle.antennaOffset) offset = vehicle.antennaOffset;
pn.fix.easting = (Math.Cos(-fixHeading) * offset) + pn.fix.easting;
pn.fix.northing = (Math.Sin(-fixHeading) * offset) + pn.fix.northing;
}
}
#endregion
#region Roll
rollUsed = 0;
if ((ahrs.isRollBrick | ahrs.isRollDogs | ahrs.isRollPAOGI) && mc.rollRaw != 9999)
{
//for charting in GPS Data window
eastingBeforeRoll = pn.fix.easting;
rollUsed = (double)mc.rollRaw/16;
//calculate how far the antenna moves based on sidehill roll
double roll = Math.Sin(glm.toRadians((mc.rollRaw - ahrs.rollZero) * 0.0625));
//change for roll to the right is positive times -1
rollCorrectionDistance = roll * vehicle.antennaHeight * -1.0;
// roll to left is positive **** important!!
// not any more - April 30, 2019 - roll to right is positive
pn.fix.easting = (Math.Cos(-fixHeading) * rollCorrectionDistance) + pn.fix.easting;
pn.fix.northing = (Math.Sin(-fixHeading) * rollCorrectionDistance) + pn.fix.northing;
//for charting the position after roll adjustment
eastingAfterRoll = pn.fix.easting;
}
else
{
eastingAfterRoll = pn.fix.easting;
eastingBeforeRoll = pn.fix.easting;
}
//pitchDistance = (pitch * vehicle.antennaHeight);
//pn.fix.easting = (Math.Sin(fixHeading) * pitchDistance) + pn.fix.easting;
//pn.fix.northing = (Math.Cos(fixHeading) * pitchDistance) + pn.fix.northing;
#endregion Roll
#region Step Fix
//**** heading of the vec3 structure is used for distance in Step fix!!!!!
//grab the most current fix and save the distance from the last fix
distanceCurrentStepFix = glm.Distance(pn.fix, stepFixPts[0]);
if (vehicle.treeSpacing != 0 && section[0].isSectionOn) treeSpacingCounter += (distanceCurrentStepFix*100);
//keep the distance below spacing
while (treeSpacingCounter > vehicle.treeSpacing && vehicle.treeSpacing != 0) treeSpacingCounter -= vehicle.treeSpacing;
fixStepDist = distanceCurrentStepFix;
//if min distance isn't exceeded, keep adding old fixes till it does
if (distanceCurrentStepFix <= minFixStepDist)
{
for (currentStepFix = 0; currentStepFix < totalFixSteps; currentStepFix++)
{
fixStepDist += stepFixPts[currentStepFix].heading;
if (fixStepDist > minFixStepDist)
{
//if we reached end, keep the oldest and stay till distance is exceeded
if (currentStepFix < (totalFixSteps - 1)) currentStepFix++;
isFixHolding = false;
break;
}
else isFixHolding = true;
}
}
// only takes a single fix to exceeed min distance
else currentStepFix = 0;
//if total distance is less then the addition of all the fixes, keep last one as reference
if (isFixHolding)
{
if (isFixHoldLoaded == false)
{
vHold = stepFixPts[(totalFixSteps - 1)];
isFixHoldLoaded = true;
}
//cycle thru like normal
for (int i = totalFixSteps - 1; i > 0; i--) stepFixPts[i] = stepFixPts[i - 1];
//fill in the latest distance and fix
stepFixPts[0].heading = glm.Distance(pn.fix, stepFixPts[0]);
stepFixPts[0].easting = pn.fix.easting;
stepFixPts[0].northing = pn.fix.northing;
//reload the last position that was triggered.
stepFixPts[(totalFixSteps - 1)].heading = glm.Distance(vHold, stepFixPts[(totalFixSteps - 1)]);
stepFixPts[(totalFixSteps - 1)].easting = vHold.easting;
stepFixPts[(totalFixSteps - 1)].northing = vHold.northing;
}
else //distance is exceeded, time to do all calcs and next frame
{
//positions and headings
CalculatePositionHeading();
//get rid of hold position
isFixHoldLoaded = false;
//don't add the total distance again
stepFixPts[(totalFixSteps - 1)].heading = 0;
//grab sentences for logging
if (isLogNMEA) { if (ct.isContourOn) { pn.logNMEASentence.Append(recvSentenceSettings); } }
//To prevent drawing high numbers of triangles, determine and test before drawing vertex
sectionTriggerDistance = glm.Distance(pn.fix, prevSectionPos);
//section on off and points, contour points
if (sectionTriggerDistance > sectionTriggerStepDistance && isJobStarted)
{
AddSectionContourPathPoints();
}
//calc distance travelled since last GPS fix
distance = glm.Distance(pn.fix, prevFix);
if ((fd.distanceUser += distance) > 3000) fd.distanceUser = 0; ;//userDistance can be reset
//most recent fixes are now the prev ones
prevFix.easting = pn.fix.easting; prevFix.northing = pn.fix.northing;
//load up history with valid data
for (int i = totalFixSteps - 1; i > 0; i--) stepFixPts[i] = stepFixPts[i - 1];
stepFixPts[0].heading = glm.Distance(pn.fix, stepFixPts[0]);
stepFixPts[0].easting = pn.fix.easting;
stepFixPts[0].northing = pn.fix.northing;
}
#endregion fix
#region AutoSteer
//preset the values
guidanceLineDistanceOff = 32000;
if (ct.isContourBtnOn)
{
ct.DistanceFromContourLine(pivotAxlePos, steerAxlePos);
}
else
{
if (curve.isCurveSet)
{
//do the calcs for AB Curve
curve.GetCurrentCurveLine(pivotAxlePos, steerAxlePos);
}
if (ABLine.isABLineSet)
{
ABLine.GetCurrentABLine(pivotAxlePos, steerAxlePos);
}
}
// autosteer at full speed of updates
if (!isAutoSteerBtnOn) //32020 means auto steer is off
{
guidanceLineDistanceOff = 32020;
}
// If Drive button enabled be normal, or just fool the autosteer and fill values
if (!ast.isInFreeDriveMode)
{
if (ahrs.isHeadingPAOGI)
{
guidanceLineSteerAngle = (Int16)(guidanceLineSteerAngle + (pn.nRoll * ((double)mc.autoSteerSettings[mc.ssKd]) * 4.166666));
}
//fill up0 the appropriate arrays with new values
mc.autoSteerData[mc.sdSpeed] = (byte)(pn.speed * 4.0);
mc.machineControlData[mc.cnSpeed] = mc.autoSteerData[mc.sdSpeed];
mc.autoSteerData[mc.sdDistanceHi] = (byte)(guidanceLineDistanceOff >> 8);
mc.autoSteerData[mc.sdDistanceLo] = (byte)guidanceLineDistanceOff;
mc.autoSteerData[mc.sdSteerAngleHi] = (byte)(guidanceLineSteerAngle >> 8);
mc.autoSteerData[mc.sdSteerAngleLo] = (byte)guidanceLineSteerAngle;
//out serial to autosteer module //indivdual classes load the distance and heading deltas
AutoSteerDataOutToPort();
}
else
{
//fill up the auto steer array with free drive values
mc.autoSteerData[mc.sdSpeed] = (byte)(pn.speed * 4.0 + 8);
mc.machineControlData[mc.cnSpeed] = mc.autoSteerData[mc.sdSpeed];
//make steer module think everything is normal
mc.autoSteerData[mc.sdDistanceHi] = (byte)(0);
mc.autoSteerData[mc.sdDistanceLo] = (byte)0;
//out serial to autosteer module //indivdual classes load the distance and heading deltas
AutoSteerDataOutToPort();
}
//for average cross track error
if (guidanceLineDistanceOff < 29000)
{
avgXTE[avgXTECntr] = Math.Abs(guidanceLineDistanceOff);
if (avgXTECntr++ > 10) avgXTECntr = 0;
crossTrackError = 0;
for (int i = 0; i < 11; i++)
{
crossTrackError += (int)avgXTE[i];
}
crossTrackError /= 10;
}
else
{
avgXTE[avgXTECntr] = 0;
if (avgXTECntr++ > 10) avgXTECntr = 0;
crossTrackError = 0;
}
#endregion
//calculate lookahead at full speed, no sentence misses
CalculateSectionLookAhead(toolPos.northing, toolPos.easting, cosSectionHeading, sinSectionHeading);
//update main window
oglMain.MakeCurrent();
oglMain.Refresh();
//end of UppdateFixPosition
}
//add the points for section, contour line points, Area Calc feature
private void AddSectionContourPathPoints()
{
if (curve.isOkToAddPoints)
{
vec3 pt = new vec3(pivotAxlePos.easting, pivotAxlePos.northing, pivotAxlePos.heading);
curve.refList.Add(pt);
}
//save the north & east as previous
prevSectionPos.northing = pn.fix.northing;
prevSectionPos.easting = pn.fix.easting;
// if non zero, at least one section is on.
int sectionCounter = 0;
//send the current and previous GPS fore/aft corrected fix to each section
for (int j = 0; j < vehicle.numOfSections + 1; j++)
{
if (section[j].isSectionOn)
{
section[j].AddPathPoint(toolPos.northing, toolPos.easting, cosSectionHeading, sinSectionHeading);
sectionCounter++;
}
}
if ((ABLine.isBtnABLineOn && !ct.isContourBtnOn && ABLine.isABLineSet && isAutoSteerBtnOn) ||
(!ct.isContourBtnOn && curve.isCurveBtnOn && curve.isCurveSet && isAutoSteerBtnOn))
{
//no contour recorded
if (ct.isContourOn)
{
ct.StopContourLine(steerAxlePos);
}
}
else
{
//if (ABLine.isABLineSet && isAutoSteerBtnOn)
//Contour Base Track.... At least One section on, turn on if not
if (sectionCounter != 0)
{
//keep the line going, everything is on for recording path
if (ct.isContourOn)
{
ct.AddPoint(pivotAxlePos);
}
else
{
ct.StartContourLine(pivotAxlePos);
ct.AddPoint(pivotAxlePos);
}
}
//All sections OFF so if on, turn off
else
{
if (ct.isContourOn)
{
ct.StopContourLine(pivotAxlePos);
}
}
//Build contour line if close enough to a patch
if (ct.isContourBtnOn)
{
ct.BuildContourGuidanceLine(pivotAxlePos);
}
}
}
