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