private void btnSnap_Click(object sender, EventArgs e)
{
ABLine.SnapABLine();
}
//called by the openglDraw routine 10 times per second.
private void UpdateFixPosition()
{
startCounter++;
//if its a valid fix data for RMC or GGA
//this is the current position taken from the latest sentence
//textBoxRcv.Text = pn.theSent;
if (!isGPSPositionInitialized)
{
InitializeFirstFewGPSPositions();
return;
}
//calc low speed distance travelled if speed < 2 kph and app not starting
if (pn.speed < 2.0) distanceLowSpeed = pn.Distance(pn.northing, pn.easting, prevLowSpeedNorthing, prevLowSpeedEasting);
else distanceLowSpeed = -1;
//calculate lookahead at full speed, no sentence misses
CalculateSectionLookAhead(toolNorthing, toolEasting, cosHeading, sinHeading);
//do the distance from line calculations for contour and AB
if (ct.isContourBtnOn) ct.DistanceFromContourLine();
else ct.distanceFromCurrentLine = 9999;
if (ABLine.isABLineSet && !ct.isContourBtnOn) ABLine.getCurrentABLine();
else ABLine.distanceFromCurrentLine = 9999;
//module communications current heading variable
//modcom.autosteerActualHeading = fixHeading;
//time to record next fix
if (distanceLowSpeed > 0.5 | pn.speed > 2.0 | startCounter < 20)
{
//positions and headings
CalculatePositionHeading();
//To prevent drawing high numbers of triangles, determine and test before drawing vertex
sectionTriggerDistance = pn.Distance(toolNorthing, toolEasting, prevSectionNorthing, prevSectionEasting);
//section on off and points, contour points
if (sectionTriggerDistance > sectionTriggerStepDistance)
AddSectionContourPathPoints();
//calc distance travelled since last GPS fix
distance = pn.Distance(pn.northing, pn.easting, prevNorthing[0], prevEasting[0]);
totalDistance += distance; //distance tally
userDistance += distance;//userDistance can be reset
//save a copy of previous positions for cam heading of desired filtering or delay
for (int x = numPrevs - 1; x > 0; x--) { prevNorthing[x] = prevNorthing[x - 1]; prevEasting[x] = prevEasting[x - 1]; }
//most recent fixes
prevEasting[0] = pn.easting; prevNorthing[0] = pn.northing;
prevToolNorthing = toolNorthing;
prevToolEasting = toolEasting;
prevFixHeadingSection = fixHeadingSection;
}
//openGLControl_Draw routine triggered manually
openGLControl.DoRender();
//end of UppdateFixPosition
}
//call for position update after valid NMEA sentence
private void UpdateFixPosition()
{
startCounter++;
totalFixSteps = fixUpdateHz * 4;
if (!isGPSPositionInitialized) { InitializeFirstFewGPSPositions(); return; }
#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));
rollCorrectionDistance = Math.Abs(roll * vehicle.antennaHeight);
// roll to left is positive **** important!!
if (roll > 0)
{
pn.fix.easting = (Math.Cos(fixHeading) * rollCorrectionDistance) + pn.fix.easting;
pn.fix.northing = (Math.Sin(fixHeading) * -rollCorrectionDistance) + pn.fix.northing;
}
else
{
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
//grab the most current fix and save the distance from the last fix
distanceCurrentStepFix = pn.Distance(pn.fix, stepFixPts[0]);
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 = pn.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 = pn.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 = pn.Distance(pn.fix, prevSectionPos);
//section on off and points, contour points
if (sectionTriggerDistance > sectionTriggerStepDistance)
{
//CRecPathPt pt = new CRecPathPt(pn.fix, fixHeading, pn.speed);
//recPath.recList.Add(pt);
AddSectionContourPathPoints();
}
//test if travelled far enough for new boundary point
double boundaryDistance = pn.Distance(pn.fix, prevBoundaryPos);
if (boundaryDistance > boundaryTriggerDistance) AddBoundaryAndPerimiterPoint();
//calc distance travelled since last GPS fix
distance = pn.Distance(pn.fix, prevFix);
if ((userDistance += distance) > 3000) userDistance = 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 = pn.Distance(pn.fix, stepFixPts[0]);
stepFixPts[0].easting = pn.fix.easting;
stepFixPts[0].northing = pn.fix.northing;
}
#endregion fix
#region AutoSteer
guidanceLineDistanceOff = 32000; //preset the values
//do the distance from line calculations for contour and AB
if (ct.isContourBtnOn) ct.DistanceFromContourLine();
if (ABLine.isABLineSet && !ct.isContourBtnOn)
{
ABLine.GetCurrentABLine();
if (yt.isRecordingCustomYouTurn)
{
//save reference of first point
if (yt.youFileList.Count == 0)
{
vec2 start = new vec2(pn.fix.easting, pn.fix.northing);
yt.youFileList.Add(start);
}
else
{
//keep adding points
vec2 point = new vec2(pn.fix.easting - yt.youFileList[0].easting, pn.fix.northing - yt.youFileList[0].northing);
yt.youFileList.Add(point);
}
}
}
// 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)
{
//fill up0 the auto steer array with new values
mc.autoSteerData[mc.sdSpeed] = (byte)(pn.speed * 4.0);
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();
SendUDPMessage(guidanceLineSteerAngle + "," + guidanceLineDistanceOff);
}
else
{
//fill up the auto steer array with free drive values
mc.autoSteerData[mc.sdSpeed] = (byte)(pn.speed * 4.0 + 8);
//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();
}
#endregion
#region relayRatecontrol
//do the relayRateControl
if (rc.isRateControlOn)
{
rc.CalculateRateLitersPerMinute();
mc.relayRateData[mc.rdRateSetPointHi] = (byte)((Int16)(rc.rateSetPoint * 100.0) >> 8);
mc.relayRateData[mc.rdRateSetPointLo] = (byte)(rc.rateSetPoint * 100.0);
mc.relayRateData[mc.rdSpeedXFour] = (byte)(pn.speed * 4.0);
//relay byte is built in SerialComm function BuildRelayByte()
//youturn control byte is built in SerialComm BuildYouTurnByte()
}
else
{
mc.relayRateData[mc.rdRateSetPointHi] = (byte)0;
mc.relayRateData[mc.rdRateSetPointHi] = (byte)0;
mc.relayRateData[mc.rdSpeedXFour] = (byte)(pn.speed * 4.0);
//relay byte is built in SerialComm.cs - function BuildRelayByte()
//youturn control byte is built in SerialComm BuildYouTurnByte()
}
//send out the port
RateRelayOutToPort(mc.relayRateData, AgOpenGPS.CModuleComm.numRelayRateDataItems);
#endregion
#region Youturn
//do the auto youturn logic every half second
if (hl.isSet && yt.isYouTurnBtnOn && isAutoSteerBtnOn)// && (youTurnCounter++ > (fixUpdateHz>>3)))
{
//figure out where we are
yt.isInBoundz = boundz.IsPointInsideBoundary(toolPos);
yt.isInWorkArea = hl.IsPointInsideHeadland(toolPos);
//Are we in the headland?
if (!yt.isInWorkArea && yt.isInBoundz) yt.isInHeadland = true;
else yt.isInHeadland = false;
//are we in boundary? Then calc a distance
if (yt.isInBoundz)
{
hl.FindClosestHeadlandPoint(pivotAxlePos);
if ((int)hl.closestHeadlandPt.easting != -1)
{
distPivot = pn.Distance(pivotAxlePos, hl.closestHeadlandPt);
}
else distPivot = -2;
}
else distPivot = -2;
//trigger the "its ready to generate a youturn when 25m away" but don't make it just yet
if (distPivot < 25.0 && distPivot > 22 && !yt.isYouTurnTriggered && yt.isInWorkArea)
{
//begin the whole process, all conditions are met
yt.YouTurnTrigger();
}
//Do the sequencing of functions around the turn.
if (yt.isSequenceTriggered)
{
yt.DoSequenceEvent();
}
distanceToStartAutoTurn = -1;
//start counting down - this is not run if shape is drawn
if (yt.isYouTurnTriggerPointSet && yt.isYouTurnBtnOn)
{
//if we are too much off track, pointing wrong way, kill the turn
if ((Math.Abs(guidanceLineSteerAngle) > 50) && (Math.Abs(ABLine.distanceFromCurrentLine) > 500))
{
yt.ResetYouTurnAndSequenceEvents();
}
else
{
//how far have we gone since youturn request was triggered
distanceToStartAutoTurn = pn.Distance(pivotAxlePos, yt.youTurnTriggerPoint);
//youTurnProgressBar = (int)(distanceToStartAutoTurn / (45 + yt.youTurnStartOffset) * 100);
if (distanceToStartAutoTurn > (25 + yt.youTurnStartOffset))
{
//keep from running this again since youturn is plotted now
yt.isYouTurnTriggerPointSet = false;
youTurnProgressBar = 0;
yt.isLastYouTurnRight = yt.isYouTurnRight;
yt.BuildYouTurnListToRight(yt.isYouTurnRight);
}
}
}
}
else //make sure youturn and sequence is off - we are not in normal turn here
{
if(yt.isYouTurnTriggered | yt.isSequenceTriggered)
{
yt.ResetYouTurnAndSequenceEvents();
}
}
#endregion
//calculate lookahead at full speed, no sentence misses
CalculateSectionLookAhead(toolPos.northing, toolPos.easting, cosSectionHeading, sinSectionHeading);
//openGLControl_Draw routine triggered manually
openGLControl.DoRender();
//end of UppdateFixPosition
}
/// Handles the OpenGLDraw event of the openGLControl control.
private void openGLControl_OpenGLDraw(object sender, RenderEventArgs e)
{
if (isGPSPositionInitialized)
{
// Get the OpenGL object.
OpenGL gl = openGLControl.OpenGL;
//System.Threading.Thread.Sleep(500);
// Clear the color and depth buffer.
gl.Clear(OpenGL.GL_COLOR_BUFFER_BIT | OpenGL.GL_DEPTH_BUFFER_BIT);
gl.LoadIdentity();
//camera does translations and rotations
camera.SetWorldCam(gl, pivotAxleEasting, fixPosY, pivotAxleNorthing, fixHeadingCam);
//calculate the frustum planes for culling
CalcFrustum(gl);
//draw the field ground images
worldGrid.DrawFieldSurface();
//Draw the world grid based on camera position
gl.Disable(OpenGL.GL_DEPTH_TEST);
gl.Disable(OpenGL.GL_TEXTURE_2D);
//if grid is on draw it
if (isGridOn)
{
worldGrid.DrawWorldGrid(gridZoom);
}
//turn on blend for paths
gl.Enable(OpenGL.GL_BLEND);
//section patch color
gl.Color(redSections, grnSections, bluSections, (byte)160);
if (isDrawPolygons)
{
gl.PolygonMode(OpenGL.GL_FRONT, OpenGL.GL_LINE);
}
//draw patches of sections
for (int j = 0; j < vehicle.numSuperSection; j++)
{
//every time the section turns off and on is a new patch
int patchCount = section[j].patchList.Count;
//check if in frustum or not
bool isDraw;
if (patchCount > 0)
{
//initialize the steps for mipmap of triangles (skipping detail while zooming out)
int mipmap = 0;
if (camera.camSetDistance < -800)
{
mipmap = 2;
}
if (camera.camSetDistance < -1500)
{
mipmap = 4;
}
if (camera.camSetDistance < -2400)
{
mipmap = 8;
}
if (camera.camSetDistance < -4800)
{
mipmap = 16;
}
//for every new chunk of patch
foreach (var triList in section[j].patchList)
{
isDraw = false;
int count2 = triList.Count;
for (int i = 0; i < count2; i += 3)
{
//determine if point is in frustum or not, if < 0, its outside so abort
if (frustum[0] * triList[i].x + frustum[2] * triList[i].z + frustum[3] <= 0)
{
continue;//right
}
if (frustum[4] * triList[i].x + frustum[6] * triList[i].z + frustum[7] <= 0)
{
continue;//left
}
if (frustum[16] * triList[i].x + frustum[18] * triList[i].z + frustum[19] <= 0)
{
continue;//bottom
}
if (frustum[20] * triList[i].x + frustum[22] * triList[i].z + frustum[23] <= 0)
{
continue;//top
}
if (frustum[8] * triList[i].x + frustum[10] * triList[i].z + frustum[11] <= 0)
{
continue;//far
}
if (frustum[12] * triList[i].x + frustum[14] * triList[i].z + frustum[15] <= 0)
{
continue;//near
}
//point is in frustum so draw the entire patch. The downside of triangle strips.
isDraw = true;
break;
}
if (isDraw)
{
//draw the triangle in each triangle strip
gl.Begin(OpenGL.GL_TRIANGLE_STRIP);
count2 = triList.Count;
//if large enough patch and camera zoomed out, fake mipmap the patches, skip triangles
if (count2 >= (mipmap + 2))
{
int step = mipmap;
for (int i = 0; i < count2; i += step)
{
gl.Vertex(triList[i].x, 0, triList[i].z); i++;
gl.Vertex(triList[i].x, 0, triList[i].z); i++;
//too small to mipmap it
if (count2 - i <= (mipmap + 2))
{
step = 0;
}
}
}
else
{
for (int i = 0; i < count2; i++)
{
gl.Vertex(triList[i].x, 0, triList[i].z);
}
}
gl.End();
}
}
}
}
gl.PolygonMode(OpenGL.GL_FRONT, OpenGL.GL_FILL);
gl.Color(1, 1, 1);
//draw contour line if button on
if (ct.isContourBtnOn)
{
ct.DrawContourLine();
}
// draw the current and reference AB Lines
else
{
if (ABLine.isABLineSet | ABLine.isABLineBeingSet)
{
ABLine.DrawABLines();
}
}
//draw the flags if there are some
int flagCnt = flagPts.Count;
if (flagCnt > 0)
{
for (int f = 0; f < flagCnt; f++)
{
gl.PointSize(8.0f);
gl.Begin(OpenGL.GL_POINTS);
if (flagPts[f].color == 0)
{
gl.Color((byte)255, (byte)0, (byte)flagPts[f].ID);
}
if (flagPts[f].color == 1)
{
gl.Color((byte)0, (byte)255, (byte)flagPts[f].ID);
}
if (flagPts[f].color == 2)
{
gl.Color((byte)255, (byte)255, (byte)flagPts[f].ID);
}
gl.Vertex(flagPts[f].easting, 0, flagPts[f].northing);
gl.End();
}
if (flagNumberPicked != 0)
{
////draw the box around flag
gl.LineWidth(4);
gl.Color(0.980f, 0.0f, 0.980f);
gl.Begin(OpenGL.GL_LINE_STRIP);
double offSet = (zoomValue * zoomValue * 0.01);
gl.Vertex(flagPts[flagNumberPicked - 1].easting, 0, flagPts[flagNumberPicked - 1].northing + offSet);
gl.Vertex(flagPts[flagNumberPicked - 1].easting - offSet, 0, flagPts[flagNumberPicked - 1].northing);
gl.Vertex(flagPts[flagNumberPicked - 1].easting, 0, flagPts[flagNumberPicked - 1].northing - offSet);
gl.Vertex(flagPts[flagNumberPicked - 1].easting + offSet, 0, flagPts[flagNumberPicked - 1].northing);
gl.Vertex(flagPts[flagNumberPicked - 1].easting, 0, flagPts[flagNumberPicked - 1].northing + offSet);
gl.End();
//draw the flag with a black dot inside
gl.PointSize(4.0f);
gl.Color(0, 0, 0);
gl.Begin(OpenGL.GL_POINTS);
gl.Vertex(flagPts[flagNumberPicked - 1].easting, 0, flagPts[flagNumberPicked - 1].northing);
gl.End();
}
}
//draw the perimter line, returns if no line to draw
periArea.DrawPerimeterLine();
//screen text for debug
//gl.DrawText(10, 15, 1, 1, 1, "Courier", 14, " lookA " + Convert.ToString(section[0].sectionLookAhead));
//gl.DrawText(10, 30, 1, 1, 1, "Courier", 14, " step " + Convert.ToString(currentStepFix));
//gl.DrawText(10, 45, 1, 1, 1, "Courier", 14, " Curr " + Convert.ToString(distanceCurrentStepFix));
//gl.DrawText(10, 60, 1, 1, 1, "Courier", 14, " dist " + Convert.ToString(fixStepDist));
//gl.DrawText(10, 75, 1, 1, 1, "Courier", 16, " DD " + Convert.ToString(dist));
//gl.DrawText(10, 90, 1, 1, 1, "Courier", 12, " t " + Convert.ToString(t));
//gl.DrawText(10, 105, 1, 0.5f, 1, "Courier", 12, " TrigSetDist(m) " + Convert.ToString(Math.Round(sectionTriggerStepDistance, 2)));
// gl.DrawText(10, 120, 1, 0.5, 1, "Courier", 12, " frame msec " + Convert.ToString((int)(frameTime)));
//draw the vehicle/implement
vehicle.DrawVehicle();
//Back to normal
gl.Color(0.98f, 0.98f, 0.98f);
gl.Disable(OpenGL.GL_BLEND);
gl.Enable(OpenGL.GL_DEPTH_TEST);
//// 2D Ortho --------------------------
gl.MatrixMode(OpenGL.GL_PROJECTION);
gl.PushMatrix();
gl.LoadIdentity();
//negative and positive on width, 0 at top to bottom ortho view
gl.Ortho2D(-(double)Width / 2, (double)Width / 2, (double)Height, 0);
// Create the appropriate modelview matrix.
gl.MatrixMode(OpenGL.GL_MODELVIEW);
gl.PushMatrix();
gl.LoadIdentity();
//draw the background when in 3D
if (camera.camPitch > -31)
{
//-10 to -32 (top) is camera pitch range. Set skybox to line up with horizon
double hite = (camera.camPitch + 32) / 22 * 0.38;
//the background
double winLeftPos = -(double)Width / 2;
double winRightPos = -winLeftPos;
gl.Enable(OpenGL.GL_TEXTURE_2D);
gl.BindTexture(OpenGL.GL_TEXTURE_2D, texture[0]); // Select Our Texture
gl.Begin(OpenGL.GL_TRIANGLE_STRIP); // Build Quad From A Triangle Strip
gl.TexCoord(0, 0); gl.Vertex(winRightPos, 0.0); // Top Right
gl.TexCoord(1, 0); gl.Vertex(winLeftPos, 0.0); // Top Left
gl.TexCoord(0, 1); gl.Vertex(winRightPos, hite * (double)Height); // Bottom Right
gl.TexCoord(1, 1); gl.Vertex(winLeftPos, hite * (double)Height); // Bottom Left
gl.End(); // Done Building Triangle Strip
//disable, straight color
gl.Disable(OpenGL.GL_TEXTURE_2D);
}
//LightBar if AB Line is set and turned on
if (isLightbarOn)
{
if (ct.isContourBtnOn)
{
txtDistanceOffABLine.Visible = true;
DrawLightBar(openGLControl.Width, openGLControl.Height, ct.distanceFromCurrentLine * 0.1);
txtDistanceOffABLine.Text = " " + Convert.ToString((int)Math.Abs(ct.distanceFromCurrentLine * 0.03937)) + " ";
if (Math.Abs(ABLine.distanceFromCurrentLine) > 15.0)
{
txtDistanceOffABLine.ForeColor = Color.Yellow;
}
else
{
txtDistanceOffABLine.ForeColor = Color.LightGreen;
}
}
else
{
if (ABLine.isABLineSet | ABLine.isABLineBeingSet)
{
txtDistanceOffABLine.Visible = true;
DrawLightBar(openGLControl.Width, openGLControl.Height, ABLine.distanceFromCurrentLine * 0.1);
txtDistanceOffABLine.Text = " " + Convert.ToString((int)Math.Abs(ABLine.distanceFromCurrentLine * 0.1)) + " ";
if (Math.Abs(ABLine.distanceFromCurrentLine) > 15.0)
{
txtDistanceOffABLine.ForeColor = Color.Yellow;
}
else
{
txtDistanceOffABLine.ForeColor = Color.LightGreen;
}
}
}
//AB line is not set so turn off numbers
if (!ABLine.isABLineSet & !ABLine.isABLineBeingSet & !ct.isContourBtnOn)
{
txtDistanceOffABLine.Visible = false;
}
}
else
{
txtDistanceOffABLine.Visible = false;
}
gl.Flush(); //finish openGL commands
gl.PopMatrix(); // Pop the modelview.
// back to the projection and pop it, then back to the model view.
gl.MatrixMode(OpenGL.GL_PROJECTION);
gl.PopMatrix();
gl.MatrixMode(OpenGL.GL_MODELVIEW);
//reset point size
gl.PointSize(1.0f);
gl.Flush();
if (leftMouseDownOnOpenGL)
{
leftMouseDownOnOpenGL = false;
byte[] data1 = new byte[192];
//scan the center of click and a set of square points around
gl.ReadPixels(mouseX - 4, mouseY - 4, 8, 8, OpenGL.GL_RGB, OpenGL.GL_UNSIGNED_BYTE, data1);
//made it here so no flag found
flagNumberPicked = 0;
for (int ctr = 0; ctr < 192; ctr += 3)
{
if (data1[ctr] == 255 | data1[ctr + 1] == 255)
{
flagNumberPicked = data1[ctr + 2];
break;
}
}
}
//draw the section control window off screen buffer
openGLControlBack.DoRender();
}
}
//call for position update after valid NMEA sentence
private void UpdateFixPosition()
{
startCounter++;
totalFixSteps = fixUpdateHz * 4;
if (!isGPSPositionInitialized) { InitializeFirstFewGPSPositions(); return; }
#region Roll
if (mc.rollRaw != 9999)
{
//calculate how far the antenna moves based on sidehill roll
double roll = Math.Sin(glm.toRadians(mc.rollRaw/16.0));
rollCorrectionDistance = Math.Abs(roll * vehicle.antennaHeight);
// tilt to left is positive **** important!!
if (roll > 0)
{
pn.easting = (Math.Cos(fixHeading) * rollCorrectionDistance) + pn.easting;
pn.northing = (Math.Sin(fixHeading) * -rollCorrectionDistance) + pn.northing;
}
else
{
pn.easting = (Math.Cos(fixHeading) * -rollCorrectionDistance) + pn.easting;
pn.northing = (Math.Sin(fixHeading) * rollCorrectionDistance) + pn.northing;
}
}
//tiltDistance = (pitch * vehicle.antennaHeight);
////pn.easting = (Math.Sin(fixHeading) * tiltDistance) + pn.easting;
//pn.northing = (Math.Cos(fixHeading) * tiltDistance) + pn.northing;
#endregion Roll
#region Step Fix
//grab the most current fix and save the distance from the last fix
distanceCurrentStepFix = pn.Distance(pn.northing, pn.easting, stepFixPts[0].northing, stepFixPts[0].easting);
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 = pn.Distance(pn.northing, pn.easting, stepFixPts[0].northing, stepFixPts[0].easting);
stepFixPts[0].easting = pn.easting;
stepFixPts[0].northing = pn.northing;
//reload the last position that was triggered.
stepFixPts[(totalFixSteps - 1)].heading = pn.Distance(vHold.northing, vHold.easting, stepFixPts[(totalFixSteps - 1)].northing, stepFixPts[(totalFixSteps - 1)].easting);
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);
}
}
//add another point if on
//AddSectionContourPathPoints();
//To prevent drawing high numbers of triangles, determine and test before drawing vertex
sectionTriggerDistance = pn.Distance(pn.northing, pn.easting, prevContourPos.northing, prevContourPos.easting);
//section on off and points, contour points
if (sectionTriggerDistance > 0.2)
{
prevContourPos.easting = pn.easting;
prevContourPos.northing = pn.northing;
AddSectionContourPathPoints();
}
//calc distance travelled since last GPS fix
distance = pn.Distance(pn.northing, pn.easting, prevFix.northing, prevFix.easting);
if ((userDistance += distance) > 9000) userDistance = 0; ;//userDistance can be reset
//most recent fixes are now the prev ones
prevFix.easting = pn.easting; prevFix.northing = pn.northing;
//load up history with valid data
for (int i = totalFixSteps - 1; i > 0; i--) stepFixPts[i] = stepFixPts[i - 1];
stepFixPts[0].heading = pn.Distance(pn.northing, pn.easting, stepFixPts[0].northing, stepFixPts[0].easting);
stepFixPts[0].easting = pn.easting;
stepFixPts[0].northing = pn.northing;
}
#endregion fix
#region AutoSteer
guidanceLineDistanceOff = 32000; //preset the values
//do the distance from line calculations for contour and AB
if (ct.isContourBtnOn) ct.DistanceFromContourLine();
if (ABLine.isABLineSet && !ct.isContourBtnOn)
{
ABLine.GetCurrentABLine();
}
// 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 (!isInFreeDriveMode)
{
//fill up0 the auto steer array with new values
mc.autoSteerData[mc.sdSpeed] = (byte)(pn.speed * 4.0);
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();
SendUDPMessage(guidanceLineSteerAngle + "," + guidanceLineDistanceOff);
}
else
{
//fill up the auto steer array with free drive values
mc.autoSteerData[mc.sdSpeed] = (byte)(pn.speed * 4.0 + 8);
//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();
}
#endregion
//openGLControl_Draw routine triggered manually
openGLControl.DoRender();
//end of UppdateFixPosition
}
/// Handles the OpenGLDraw event of the openGLControl control.
private void openGLControl_OpenGLDraw(object sender, RenderEventArgs e)
{
if (isGPSPositionInitialized)
{
// Get the OpenGL object.
OpenGL gl = openGLControl.OpenGL;
//System.Threading.Thread.Sleep(500);
// Clear the color and depth buffer.
gl.Clear(OpenGL.GL_COLOR_BUFFER_BIT | OpenGL.GL_DEPTH_BUFFER_BIT);
gl.LoadIdentity();
//camera does translations and rotations
camera.SetWorldCam(gl, pn.easting, pn.northing, camHeading);
//draw the field ground images
worldGrid.DrawFieldSurface();
////Draw the world grid based on camera position
gl.Disable(OpenGL.GL_DEPTH_TEST);
gl.Disable(OpenGL.GL_TEXTURE_2D);
gl.Enable(OpenGL.GL_LINE_SMOOTH);
gl.Enable(OpenGL.GL_BLEND);
gl.Hint(OpenGL.GL_LINE_SMOOTH_HINT, OpenGL.GL_FASTEST);
gl.Hint(OpenGL.GL_POINT_SMOOTH_HINT, OpenGL.GL_FASTEST);
gl.Hint(OpenGL.GL_POLYGON_SMOOTH_HINT, OpenGL.GL_FASTEST);
////if grid is on draw it
if (isGridOn)
{
worldGrid.DrawWorldGrid(gridZoom);
}
//turn on blend for paths
gl.Enable(OpenGL.GL_BLEND);
//section patch color
gl.Color(redSections, grnSections, bluSections, (byte)160);
if (isDrawPolygons)
{
gl.PolygonMode(OpenGL.GL_FRONT, OpenGL.GL_LINE);
}
gl.PolygonMode(OpenGL.GL_FRONT, OpenGL.GL_FILL);
gl.Color(1, 1, 1);
//draw contour line if button on
//if (ct.isContourBtnOn)
// draw the current and reference AB Lines
if (ABLine.isABLineSet | ABLine.isABLineBeingSet)
{
ABLine.DrawABLines();
}
else
{
ct.DrawContourLine();
}
//draw the flags if there are some
int flagCnt = flagPts.Count;
if (flagCnt > 0)
{
for (int f = 0; f < flagCnt; f++)
{
gl.PointSize(8.0f);
gl.Begin(OpenGL.GL_POINTS);
if (flagPts[f].color == 0)
{
gl.Color((byte)255, (byte)0, (byte)flagPts[f].ID);
}
if (flagPts[f].color == 1)
{
gl.Color((byte)0, (byte)255, (byte)flagPts[f].ID);
}
if (flagPts[f].color == 2)
{
gl.Color((byte)255, (byte)255, (byte)flagPts[f].ID);
}
gl.Vertex(flagPts[f].easting, flagPts[f].northing, 0);
gl.End();
}
if (flagNumberPicked != 0)
{
////draw the box around flag
gl.LineWidth(4);
gl.Color(0.980f, 0.0f, 0.980f);
gl.Begin(OpenGL.GL_LINE_STRIP);
double offSet = (zoomValue * zoomValue * 0.01);
gl.Vertex(flagPts[flagNumberPicked - 1].easting, flagPts[flagNumberPicked - 1].northing + offSet, 0);
gl.Vertex(flagPts[flagNumberPicked - 1].easting - offSet, flagPts[flagNumberPicked - 1].northing, 0);
gl.Vertex(flagPts[flagNumberPicked - 1].easting, flagPts[flagNumberPicked - 1].northing - offSet, 0);
gl.Vertex(flagPts[flagNumberPicked - 1].easting + offSet, flagPts[flagNumberPicked - 1].northing, 0);
gl.Vertex(flagPts[flagNumberPicked - 1].easting, flagPts[flagNumberPicked - 1].northing + offSet, 0);
gl.End();
//draw the flag with a black dot inside
gl.PointSize(4.0f);
gl.Color(0, 0, 0);
gl.Begin(OpenGL.GL_POINTS);
gl.Vertex(flagPts[flagNumberPicked - 1].easting, flagPts[flagNumberPicked - 1].northing, 0);
gl.End();
}
}
//screen text for debug
//gl.DrawText(120, 10, 1, 1, 1, "Courier Bold", 18, "Head: " + saveCounter.ToString("N1"));
//gl.DrawText(120, 40, 1, 1, 1, "Courier Bold", 18, "Tool: " + distTool.ToString("N1"));
//gl.DrawText(120, 70, 1, 1, 1, "Courier Bold", 18, "Where: " + yt.whereAmI.ToString());
//gl.DrawText(120, 100, 1, 1, 1, "Courier Bold", 18, "Seq: " + yt.isSequenceTriggered.ToString());
//gl.DrawText(120, 40, 1, 1, 1, "Courier Bold", 18, " GPS: " + Convert.ToString(Math.Round(glm.toDegrees(gpsHeading), 2)));
//gl.DrawText(120, 70, 1, 1, 1, "Courier Bold", 18, "Fixed: " + Convert.ToString(Math.Round(glm.toDegrees(gyroCorrected), 2)));
//gl.DrawText(120, 100, 1, 1, 1, "Courier Bold", 18, "L/Min: " + Convert.ToString(rc.CalculateRateLitersPerMinute()));
//gl.DrawText(120, 130, 1, 1, 1, "Courier", 18, " Roll: " + Convert.ToString(glm.toDegrees(rollDistance)));
//gl.DrawText(120, 160, 1, 1, 1, "Courier", 18, " Turn: " + Convert.ToString(Math.Round(turnDelta, 4)));
//gl.DrawText(40, 120, 1, 0.5, 1, "Courier", 12, " frame msec " + Convert.ToString((int)(frameTime)));
//draw the vehicle/implement
vehicle.DrawVehicle();
//Back to normal
gl.Color(0.98f, 0.98f, 0.98f);
gl.Disable(OpenGL.GL_BLEND);
gl.Enable(OpenGL.GL_DEPTH_TEST);
//// 2D Ortho --------------------------
gl.MatrixMode(OpenGL.GL_PROJECTION);
gl.PushMatrix();
gl.LoadIdentity();
//negative and positive on width, 0 at top to bottom ortho view
gl.Ortho2D(-(double)Width / 2, (double)Width / 2, (double)Height, 0);
// Create the appropriate modelview matrix.
gl.MatrixMode(OpenGL.GL_MODELVIEW);
gl.PushMatrix();
gl.LoadIdentity();
if (isSkyOn)
{
////draw the background when in 3D
if (camera.camPitch < -60)
{
//-10 to -32 (top) is camera pitch range. Set skybox to line up with horizon
double hite = (camera.camPitch + 60) / -20 * 0.34;
//hite = 0.001;
//the background
double winLeftPos = -(double)Width / 2;
double winRightPos = -winLeftPos;
gl.Enable(OpenGL.GL_TEXTURE_2D);
gl.BindTexture(OpenGL.GL_TEXTURE_2D, texture[0]); // Select Our Texture
gl.Begin(OpenGL.GL_TRIANGLE_STRIP); // Build Quad From A Triangle Strip
gl.TexCoord(0, 0); gl.Vertex(winRightPos, 0.0); // Top Right
gl.TexCoord(1, 0); gl.Vertex(winLeftPos, 0.0); // Top Left
gl.TexCoord(0, 1); gl.Vertex(winRightPos, hite * (double)Height); // Bottom Right
gl.TexCoord(1, 1); gl.Vertex(winLeftPos, hite * (double)Height); // Bottom Left
gl.End(); // Done Building Triangle Strip
//disable, straight color
gl.Disable(OpenGL.GL_TEXTURE_2D);
}
}
//LightBar if AB Line is set and turned on or contour
if (isLightbarOn)
{
if (ct.isContourBtnOn)
{
string dist;
txtDistanceOffABLine.Visible = true;
//lblDelta.Visible = true;
if (ct.distanceFromCurrentLine == 32000)
{
ct.distanceFromCurrentLine = 0;
}
DrawLightBar(openGLControl.Width, openGLControl.Height, ct.distanceFromCurrentLine * 0.1);
if ((ct.distanceFromCurrentLine) < 0.0)
{
txtDistanceOffABLine.ForeColor = Color.Green;
if (isMetric)
{
dist = ((int)Math.Abs(ct.distanceFromCurrentLine * 0.1)) + " ->";
}
else
{
dist = ((int)Math.Abs(ct.distanceFromCurrentLine / 2.54 * 0.1)) + " ->";
}
txtDistanceOffABLine.Text = dist;
}
else
{
txtDistanceOffABLine.ForeColor = Color.Red;
if (isMetric)
{
dist = "<- " + ((int)Math.Abs(ct.distanceFromCurrentLine * 0.1));
}
else
{
dist = "<- " + ((int)Math.Abs(ct.distanceFromCurrentLine / 2.54 * 0.1));
}
txtDistanceOffABLine.Text = dist;
}
//if (guidanceLineHeadingDelta < 0) lblDelta.ForeColor = Color.Red;
//else lblDelta.ForeColor = Color.Green;
if (guidanceLineDistanceOff == 32020 | guidanceLineDistanceOff == 32000)
{
btnAutoSteer.Text = "-";
}
else
{
btnAutoSteer.Text = "Y";
}
}
else
{
if (ABLine.isABLineSet | ABLine.isABLineBeingSet)
{
string dist;
txtDistanceOffABLine.Visible = true;
//lblDelta.Visible = true;
DrawLightBar(openGLControl.Width, openGLControl.Height, ABLine.distanceFromCurrentLine * 0.1);
if ((ABLine.distanceFromCurrentLine) < 0.0)
{
// --->
txtDistanceOffABLine.ForeColor = Color.Green;
if (isMetric)
{
dist = ((int)Math.Abs(ABLine.distanceFromCurrentLine * 0.1)) + " ->";
}
else
{
dist = ((int)Math.Abs(ABLine.distanceFromCurrentLine / 2.54 * 0.1)) + " ->";
}
txtDistanceOffABLine.Text = dist;
}
else
{
// <----
txtDistanceOffABLine.ForeColor = Color.Red;
if (isMetric)
{
dist = "<- " + ((int)Math.Abs(ABLine.distanceFromCurrentLine * 0.1));
}
else
{
dist = "<- " + ((int)Math.Abs(ABLine.distanceFromCurrentLine / 2.54 * 0.1));
}
txtDistanceOffABLine.Text = dist;
}
//if (guidanceLineHeadingDelta < 0) lblDelta.ForeColor = Color.Red;
//else lblDelta.ForeColor = Color.Green;
if (guidanceLineDistanceOff == 32020 | guidanceLineDistanceOff == 32000)
{
btnAutoSteer.Text = "-";
}
else
{
btnAutoSteer.Text = "Y";
}
}
}
//AB line is not set so turn off numbers
if (!ABLine.isABLineSet & !ABLine.isABLineBeingSet & !ct.isContourBtnOn)
{
txtDistanceOffABLine.Visible = false;
btnAutoSteer.Text = "-";
}
}
else
{
txtDistanceOffABLine.Visible = false;
btnAutoSteer.Text = "-";
}
gl.Flush(); //finish openGL commands
gl.PopMatrix(); // Pop the modelview.
// back to the projection and pop it, then back to the model view.
gl.MatrixMode(OpenGL.GL_PROJECTION);
gl.PopMatrix();
gl.MatrixMode(OpenGL.GL_MODELVIEW);
//reset point size
gl.PointSize(1.0f);
gl.Flush();
if (leftMouseDownOnOpenGL)
{
leftMouseDownOnOpenGL = false;
byte[] data1 = new byte[192];
//scan the center of click and a set of square points around
gl.ReadPixels(mouseX - 4, mouseY - 4, 8, 8, OpenGL.GL_RGB, OpenGL.GL_UNSIGNED_BYTE, data1);
//made it here so no flag found
flagNumberPicked = 0;
for (int ctr = 0; ctr < 192; ctr += 3)
{
if (data1[ctr] == 255 | data1[ctr + 1] == 255)
{
flagNumberPicked = data1[ctr + 2];
break;
}
}
}
//digital input Master control (WorkSwitch)
if (isJobStarted && mc.isWorkSwitchEnabled)
{
//check condition of work switch
if (mc.isWorkSwitchActiveLow)
{
//if (mc.workSwitchValue == 0)
}
else
{
//if (mc.workSwitchValue == 1)
}
}
//stop the timer and calc how long it took to do calcs and draw
frameTime = (double)swFrame.ElapsedTicks / (double)System.Diagnostics.Stopwatch.Frequency * 1000;
//if a couple minute has elapsed save the field in case of crash and to be able to resume
if (saveCounter > 60) //2 counts per second X 60 seconds = 120 counts per minute.
{
if (isJobStarted && stripOnlineGPS.Value != 1)
{
//auto save the field patches, contours accumulated so far
FileSaveField();
//FileSaveContour();
//NMEA log file
if (isLogNMEA)
{
FileSaveNMEA();
}
}
saveCounter = 0;
}
openGLControlBack.DoRender();
}
}
//call for position update after valid NMEA sentence
private void UpdateFixPosition()
{
startCounter++;
totalFixSteps = fixUpdateHz * 4;
if (!isGPSPositionInitialized) { InitializeFirstFewGPSPositions(); return; }
#region Roll
if (mc.rollRaw != 9999)
{
//calculate how far the antenna moves based on sidehill roll
double roll = Math.Sin(glm.toRadians(mc.rollRaw/16.0));
rollCorrectionDistance = Math.Abs(roll * vehicle.antennaHeight);
// tilt to left is positive **** important!!
if (roll > 0)
{
pn.easting = (Math.Cos(fixHeading) * rollCorrectionDistance) + pn.easting;
pn.northing = (Math.Sin(fixHeading) * -rollCorrectionDistance) + pn.northing;
}
else
{
pn.easting = (Math.Cos(fixHeading) * -rollCorrectionDistance) + pn.easting;
pn.northing = (Math.Sin(fixHeading) * rollCorrectionDistance) + pn.northing;
}
}
//tiltDistance = (pitch * vehicle.antennaHeight);
////pn.easting = (Math.Sin(fixHeading) * tiltDistance) + pn.easting;
//pn.northing = (Math.Cos(fixHeading) * tiltDistance) + pn.northing;
#endregion Roll
#region Step Fix
//grab the most current fix and save the distance from the last fix
distanceCurrentStepFix = pn.Distance(pn.northing, pn.easting, stepFixPts[0].northing, stepFixPts[0].easting);
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 = pn.Distance(pn.northing, pn.easting, stepFixPts[0].northing, stepFixPts[0].easting);
stepFixPts[0].easting = pn.easting;
stepFixPts[0].northing = pn.northing;
//reload the last position that was triggered.
stepFixPts[(totalFixSteps - 1)].heading = pn.Distance(vHold.northing, vHold.easting, stepFixPts[(totalFixSteps - 1)].northing, stepFixPts[(totalFixSteps - 1)].easting);
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 = pn.Distance(pn.northing, pn.easting, prevSectionPos.northing, prevSectionPos.easting);
//section on off and points, contour points
if (sectionTriggerDistance > sectionTriggerStepDistance)
AddSectionContourPathPoints();
//test if travelled far enough for new boundary point
double boundaryDistance = pn.Distance(pn.northing, pn.easting, prevBoundaryPos.northing, prevBoundaryPos.easting);
if (boundaryDistance > boundaryTriggerDistance) AddBoundaryAndPerimiterPoint();
//calc distance travelled since last GPS fix
distance = pn.Distance(pn.northing, pn.easting, prevFix.northing, prevFix.easting);
if ((userDistance += distance) > 3000) userDistance = 0; ;//userDistance can be reset
//most recent fixes are now the prev ones
prevFix.easting = pn.easting; prevFix.northing = pn.northing;
//load up history with valid data
for (int i = totalFixSteps - 1; i > 0; i--) stepFixPts[i] = stepFixPts[i - 1];
stepFixPts[0].heading = pn.Distance(pn.northing, pn.easting, stepFixPts[0].northing, stepFixPts[0].easting);
stepFixPts[0].easting = pn.easting;
stepFixPts[0].northing = pn.northing;
}
#endregion fix
#region AutoSteer
guidanceLineDistanceOff = 32000; //preset the values
//do the distance from line calculations for contour and AB
if (ct.isContourBtnOn) ct.DistanceFromContourLine();
if (ABLine.isABLineSet && !ct.isContourBtnOn)
{
ABLine.GetCurrentABLine();
if (yt.isRecordingYouTurn)
{
//save reference of first point
if (yt.youFileList.Count == 0)
{
vec2 start = new vec2(pn.easting, pn.northing);
yt.youFileList.Add(start);
}
else
{
//keep adding points
vec2 point = new vec2(pn.easting - yt.youFileList[0].easting, pn.northing - yt.youFileList[0].northing);
yt.youFileList.Add(point);
}
}
}
// 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 (!isInFreeDriveMode)
{
//fill up0 the auto steer array with new values
mc.autoSteerData[mc.sdSpeed] = (byte)(pn.speed * 4.0);
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();
SendUDPMessage(guidanceLineSteerAngle + "," + guidanceLineDistanceOff);
}
else
{
//fill up the auto steer array with free drive values
mc.autoSteerData[mc.sdSpeed] = (byte)(pn.speed * 4.0 + 8);
//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();
}
#endregion
//do the relayRateControl
if (rc.isRateControlOn)
{
rc.CalculateRateLitersPerMinute();
mc.relayRateData[mc.rdRateSetPointHi] = (byte)((Int16)(rc.rateSetPoint * 100.0) >> 8);
mc.relayRateData[mc.rdRateSetPointLo] = (byte)(rc.rateSetPoint * 100.0);
mc.relayRateData[mc.rdSpeedXFour] = (byte)(pn.speed * 4.0);
//relay byte is built in SerialComm function BuildRelayByte()
}
else
{
mc.relayRateData[mc.rdRateSetPointHi] = (byte)0;
mc.relayRateData[mc.rdRateSetPointHi] = (byte)0;
mc.relayRateData[mc.rdSpeedXFour] = (byte)(pn.speed * 4.0);
//relay byte is built in SerialComm fx BuildRelayByte()
}
//send out the port
RateRelayOutToPort(mc.relayRateData, AgOpenGPS.CModuleComm.numRelayRateDataItems);
//calculate lookahead at full speed, no sentence misses
CalculateSectionLookAhead(toolPos.northing, toolPos.easting, cosSectionHeading, sinSectionHeading);
//do the youturn logic every half second
if (boundary.isSet && (youTurnCounter++ > (fixUpdateHz>>2)))
{
//reset the counter
youTurnCounter = 0;
//are we in boundary? Then calc a distance
if (boundary.IsPrePointInPolygon(pivotAxlePos))
{
bool isVehicleInsideBoundary = boundary.IsPrePointInPolygon(pivotAxlePos);
if (isVehicleInsideBoundary)
{
boundary.FindClosestBoundaryPoint(pivotAxlePos);
if ((int)boundary.closestBoundaryPt.easting != -1)
{
distPt = pn.Distance(pivotAxlePos.northing, pivotAxlePos.easting, boundary.closestBoundaryPt.northing, boundary.closestBoundaryPt.easting);
}
else distPt = -2;
}
else distPt = -2;
}
else distPt = -2;
//trigger the "its ready to generate a youturn when 50m away" but don't make it just yet
if (distPt < 50.0 && distPt > 40 && !yt.isAutoTriggered && yt.isAutoYouTurnEnabled)
{
yt.isAutoTriggered = true;
//data buffer for pixels read from off screen buffer
byte[] grnPix = new byte[401];
//read a pixel line across full buffer width
OpenGL gl = openGLControlBack.OpenGL;
gl.ReadPixels(0, 205, 399, 1, OpenGL.GL_GREEN, OpenGL.GL_UNSIGNED_BYTE, grnPix);
//set up the positions to scan in the array for applied
int leftPos = vehicle.rpXPosition - 15;
if (leftPos < 0) leftPos = 0;
int rightPos = vehicle.rpXPosition + vehicle.rpWidth + 15;
if (rightPos > 399) rightPos = 399;
//do we need a left or right turn
bool isGrnOnLeft = false, isGrnOnRight = false;
//green on left means turn right
for (int j = leftPos; j < vehicle.rpXPosition; j++)
{ if (grnPix[j] > 50) isGrnOnLeft = true; else isGrnOnLeft = false; }
//green on right means turn left
for (int j = (rightPos - 10); j < rightPos; j++)
{ if (grnPix[j] > 50) isGrnOnRight = true; else isGrnOnRight = false; }
//one side or the other - but not both
if (!isGrnOnLeft && isGrnOnRight || isGrnOnLeft && !isGrnOnRight)
{
//set point and save to start measuring from
yt.isAutoPointSet = true;
yt.autoYouTurnTriggerPoint = pivotAxlePos;
if (isGrnOnRight) yt.isAutoTurnRight = false;
else yt.isAutoTurnRight = true;
}
//can't determine which way to turn, so pick opposite of last turn
else
{
yt.isAutoPointSet = true;
yt.autoYouTurnTriggerPoint = pivotAxlePos;
//just do the opposite of last turn
yt.isAutoTurnRight = !yt.isLastAutoTurnRight;
yt.isLastAutoTurnRight = !yt.isLastAutoTurnRight;
}
//modify the buttons to show the correct turn direction
if (yt.isAutoTurnRight)
{
AutoYouTurnButtonsRightTurn(); }
else
{
AutoYouTurnButtonsLeftTurn();
}
}
distanceToStartAutoTurn = -1;
//start counting down
if (yt.isAutoPointSet && yt.isAutoYouTurnEnabled)
{
//if we are too much off track, pointing wrong way, kill the turn
if ((Math.Abs(guidanceLineSteerAngle) > 50) && (Math.Abs(ABLine.distanceFromCurrentLine) > 500))
{
yt.CancelYouTurn();
distanceToStartAutoTurn = -1;
autoTurnInProgressBar = 0;
AutoYouTurnButtonsReset();
}
else
{
//how far have we gone since youturn request was triggered
distanceToStartAutoTurn = pn.Distance(pivotAxlePos.northing, pivotAxlePos.easting, yt.autoYouTurnTriggerPoint.northing, yt.autoYouTurnTriggerPoint.easting);
autoTurnInProgressBar = (int)(distanceToStartAutoTurn / (50 + yt.startYouTurnAt) * 100);
if (distanceToStartAutoTurn > (50 + yt.startYouTurnAt))
{
//keep from running this again since youturn is plotted now
yt.isAutoPointSet = false;
autoTurnInProgressBar = 0;
yt.isLastAutoTurnRight = yt.isAutoTurnRight;
yt.BuildYouTurnListToRight(yt.isAutoTurnRight);
}
}
}
}
//openGLControl_Draw routine triggered manually
openGLControl.DoRender();
//end of UppdateFixPosition
}
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
}
//call for position update after valid NMEA sentence
private void UpdateFixPosition()
{
startCounter++;
totalFixSteps = fixUpdateHz * 4;
if (!isGPSPositionInitialized) { InitializeFirstFewGPSPositions(); return; }
#region tilt
//average out angular velocity
int times = 30;
avgAngularVelocity[ringCounterTiltRoll] = modcom.angularVelocity;
if (ringCounterAngularVelocity++ == times) ringCounterAngularVelocity = 0;
angVel = 0;
for (int c = 0; c <= times - 1; c++) angVel += avgAngularVelocity[c];
angVel /= times;
avgAngVel = Math.Round(angVel, 1);
//average out the roll angle
times = 5;
avgTiltRoll[ringCounterTiltRoll] = modcom.rollAngle+rollZero;
if (ringCounterTiltRoll++ == times) ringCounterTiltRoll = 0;
roll = 0;
for (int c = 0; c <= times - 1; c++) roll += avgTiltRoll[c];
roll /= times;
avgRoll = Math.Round(roll, 1);
//Convert 16 bit int to degrees, take the sin of it
roll = Math.Sin(glm.toRadians(roll));
tiltDistance = Math.Abs(roll * vehicle.antennaHeight);
////tilt to left is positive
//if (roll > 0)
//{
// pn.easting = (Math.Cos(fixHeading) * tiltDistance) + pn.easting;
// pn.northing = (Math.Sin(fixHeading) * -tiltDistance) + pn.northing;
//}
//else
//{
// pn.easting = (Math.Cos(fixHeading) * -tiltDistance) + pn.easting;
// pn.northing = (Math.Sin(fixHeading) * tiltDistance) + pn.northing;
//}
//average out the pitch angle
times = 5;
avgTiltPitch[ringCounterTiltPitch] = modcom.pitchAngle+pitchZero;
if (ringCounterTiltPitch++ == times - 1) ringCounterTiltPitch = 0;
pitch = 0;
for (int c = 0; c < times; c++) pitch += avgTiltPitch[c];
pitch /= times;
avgPitch = Math.Round(pitch, 1);
//Convert 16 bit int to degrees, take the sin of it
pitch = Math.Sin(glm.toRadians(pitch));
tiltDistance = (pitch * vehicle.antennaHeight);
//pn.easting = (Math.Sin(fixHeading) * tiltDistance) + pn.easting;
//pn.northing = (Math.Cos(fixHeading) * tiltDistance) + pn.northing;
#endregion
#region Step Fix
distanceCurrentStepFix = pn.Distance(pn.northing, pn.easting, stepFixPts[0].z, stepFixPts[0].x);
fixStepDist = distanceCurrentStepFix;
if (distanceCurrentStepFix <= minFixStepDist)
{
for (currentStepFix = 0; currentStepFix < totalFixSteps; currentStepFix++)
{
fixStepDist += stepFixPts[currentStepFix].h;
if (fixStepDist > minFixStepDist)
{
if (currentStepFix < (totalFixSteps-1) ) currentStepFix++;
isFixHolding = false;
break;
}
else isFixHolding = true;
}
}
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].h = pn.Distance(pn.northing, pn.easting, stepFixPts[0].z, stepFixPts[0].x);
stepFixPts[0].x = pn.easting;
stepFixPts[0].z = pn.northing;
//reload the last position that was triggered.
stepFixPts[(totalFixSteps - 1)].h = pn.Distance(vHold.z, vHold.x, stepFixPts[(totalFixSteps - 1)].z, stepFixPts[(totalFixSteps - 1)].x);
stepFixPts[(totalFixSteps - 1)].x = vHold.x;
stepFixPts[(totalFixSteps - 1)].z = vHold.z;
}
else
{
//positions and headings
CalculatePositionHeading();
isFixHoldLoaded = false;
stepFixPts[(totalFixSteps - 1)].h = 0;
//To prevent drawing high numbers of triangles, determine and test before drawing vertex
sectionTriggerDistance = pn.Distance(toolNorthing, toolEasting, prevSectionNorthing, prevSectionEasting);
//section on off and points, contour points
if (sectionTriggerDistance > sectionTriggerStepDistance)
AddSectionContourPathPoints();
//calc distance travelled since last GPS fix
distance = pn.Distance(pn.northing, pn.easting, prevNorthing, prevEasting);
totalDistance += distance; //distance tally
userDistance += distance;//userDistance can be reset
//most recent fixes
prevEasting = pn.easting; prevNorthing = pn.northing;
//load up history with valid data
for (int i = totalFixSteps - 1; i > 0; i--) stepFixPts[i] = stepFixPts[i - 1];
stepFixPts[0].h = pn.Distance(pn.northing, pn.easting, stepFixPts[0].z, stepFixPts[0].x);
stepFixPts[0].x = pn.easting;
stepFixPts[0].z = pn.northing;
}
#endregion
#region AutoSteer
//preset the values
guidanceLineDistanceOff = 32000;
guidanceLineHeadingDelta = 0;
//do the distance from line calculations for contour and AB
if (ct.isContourBtnOn) ct.DistanceFromContourLine();
if (ABLine.isABLineSet && !ct.isContourBtnOn) ABLine.getCurrentABLine();
// autosteer at full speed of updates
if (!isAutoSteerBtnOn)
{
guidanceLineDistanceOff = 32020;
guidanceLineHeadingDelta = 32020;
}
modcom.autoSteerControl[0] = (byte)(guidanceLineDistanceOff >> 8);
modcom.autoSteerControl[1] = (byte)guidanceLineDistanceOff;
modcom.autoSteerControl[2] = (byte)(guidanceLineHeadingDelta >> 8);
modcom.autoSteerControl[3] = (byte)guidanceLineHeadingDelta;
//out serial to autosteer module //indivdual classes load the distance and heading deltas
AutoSteerControlOutToPort();
#endregion
//calculate lookahead at full speed, no sentence misses
CalculateSectionLookAhead(toolNorthing, toolEasting, cosSectionHeading, sinSectionHeading);
//openGLControl_Draw routine triggered manually
openGLControl.DoRender();
//end of UppdateFixPosition
}
/// Handles the OpenGLDraw event of the openGLControl control.
private void openGLControl_OpenGLDraw(object sender, RenderEventArgs e)
{
if (isGPSPositionInitialized)
{
// Get the OpenGL object.
OpenGL gl = openGLControl.OpenGL;
//System.Threading.Thread.Sleep(500);
// Clear the color and depth buffer.
gl.Clear(OpenGL.GL_COLOR_BUFFER_BIT | OpenGL.GL_DEPTH_BUFFER_BIT);
gl.LoadIdentity();
//camera does translations and rotations
camera.SetWorldCam(gl, pivotAxleEasting, fixPosY, pivotAxleNorthing, fixHeadingCam);
//calculate the frustum planes for culling
CalcFrustum(gl);
worldGrid.DrawFieldSurface();
//Draw the world grid based on camera position
gl.Disable(OpenGL.GL_DEPTH_TEST);
gl.Disable(OpenGL.GL_TEXTURE_2D);
//if grid is on draw it
if (isGridOn)
{
worldGrid.DrawWorldGrid(gridZoom);
}
//turn on blend for paths
gl.Enable(OpenGL.GL_BLEND);
//section patch color
gl.Color(0.99f, 1.0f, 0.50f, 0.4f);
if (isDrawPolygons)
{
gl.PolygonMode(OpenGL.GL_FRONT, OpenGL.GL_LINE);
}
triDrawn = 0;
patchesDrawn = 0;
totalTri = 0;
//draw patches of sections
for (int j = 0; j < vehicle.numberOfSections; j++)
{
//every time the section turns off and on is a new patch
int patchCount = section[j].patchList.Count;
//check if in frustum or not
bool isDraw;
if (patchCount > 0)
{
//initialize the steps for mipmap of triangles (skipping detail while zooming out)
int mipmap = 0;
if (camera.camSetDistance < -800)
{
mipmap = 2;
}
if (camera.camSetDistance < -1500)
{
mipmap = 4;
}
if (camera.camSetDistance < -2400)
{
mipmap = 8;
}
if (camera.camSetDistance < -4800)
{
mipmap = 16;
}
//for every new chunk of patch
foreach (var triList in section[j].patchList)
{
isDraw = false;
int count2 = triList.Count;
totalTri += count2;
for (int i = 0; i < count2; i += 3)
{
//determine if point is in frustum or not, if < 0, its outside so abort
if (frustum[0] * triList[i].x + frustum[2] * triList[i].z + frustum[3] <= 0)
{
continue;//right
}
if (frustum[4] * triList[i].x + frustum[6] * triList[i].z + frustum[7] <= 0)
{
continue;//left
}
if (frustum[16] * triList[i].x + frustum[18] * triList[i].z + frustum[19] <= 0)
{
continue;//bottom
}
if (frustum[20] * triList[i].x + frustum[22] * triList[i].z + frustum[23] <= 0)
{
continue;//top
}
if (frustum[8] * triList[i].x + frustum[10] * triList[i].z + frustum[11] <= 0)
{
continue;//far
}
if (frustum[12] * triList[i].x + frustum[14] * triList[i].z + frustum[15] <= 0)
{
continue;//near
}
//point is in frustum so draw the entire patch. The downside of triangle strips.
isDraw = true;
break;
}
if (isDraw)
{
patchesDrawn++;
//draw the triangle strip in each triangle strip
gl.Begin(OpenGL.GL_TRIANGLE_STRIP);
count2 = triList.Count;
//if large enough patch and camera zoomed out, fake mipmap the patches, skip triangles
if (count2 >= (mipmap + 2))
{
int step = mipmap;
for (int i = 0; i < count2; i += step)
{
gl.Vertex(triList[i].x, 0, triList[i].z);
triDrawn++; i++;
gl.Vertex(triList[i].x, 0, triList[i].z);
triDrawn++; i++;
//too small to mipmap it
if (count2 - i <= (mipmap + 2))
{
step = 0;
}
}
}
else
{
for (int i = 0; i < count2; i++)
{
gl.Vertex(triList[i].x, 0, triList[i].z);
triDrawn++;
}
}
gl.End();
}
}
}
}
gl.PolygonMode(OpenGL.GL_FRONT, OpenGL.GL_FILL);
gl.Color(1, 1, 1);
//draw contour line if button on
if (ct.isContourBtnOn)
{
ct.DrawContourLine();
}
// draw the current and reference AB Lines
else
{
if (ABLine.isABLineSet | ABLine.isABLineBeingSet)
{
ABLine.DrawABLines();
}
}
//draw the perimter line, returns if no line to draw
periArea.DrawPerimeterLine();
//screen text for debug
//gl.DrawText(10, 15, 1, 1, 1, "Courier", 14, " frame msec " + Convert.ToString((int)(frameTime)));
// gl.DrawText(10, 30, 1, 1, 1, "Courier", 14, "recvC " + Convert.ToString(recvCounter));
// gl.DrawText(10, 45, 1, 1, 1, "Courier", 14, " Set Head " + Convert.ToString(modcom.autosteerSetpointHeading));
// gl.DrawText(10, 60, 1, 1, 1, "Courier", 14, " Actual " + Convert.ToString(modcom.autosteerActualHeading));
////gl.DrawText(10, 75, 1, 0.5f, 1, "Courier", 12, "refHeading " + Convert.ToString(Math.Round(ct.refHeading, 2)));
//gl.DrawText(10, 90, 1, 0.5f, 1, "Courier", 12, "Lookahead[0] (m) " + Convert.ToString(Math.Round(section[0].sectionLookAhead*0.1)));
//gl.DrawText(10, 105, 1, 0.5f, 1, "Courier", 12, " TrigDistance(m) " + Convert.ToString(Math.Round(sectionTriggerStepDistance, 2)));
//gl.DrawText(10, 120, 1, 0.5, 1, "Courier", 12, " frame msec " + Convert.ToString((int)(frameTime)));
//draw the tractor/implement
vehicle.DrawVehicle();
//Back to normal
gl.Color(1.0f, 1.0f, 1.0f);
gl.Disable(OpenGL.GL_BLEND);
gl.Enable(OpenGL.GL_DEPTH_TEST);
//// 2D Ortho --------------------------
gl.MatrixMode(OpenGL.GL_PROJECTION);
gl.PushMatrix();
gl.LoadIdentity();
//negative and positive on width, 0 at top to bottom ortho view
gl.Ortho2D(-(double)Width / 2, (double)Width / 2, (double)Height, 0);
// Create the appropriate modelview matrix.
gl.MatrixMode(OpenGL.GL_MODELVIEW);
gl.PushMatrix();
gl.LoadIdentity();
//draw the background when in 3D
if (isIn3D && camera.camPitch > -16)
{
//the background
double winLeftPos = -(double)Width / 2;
double winRightPos = -winLeftPos;
gl.Enable(OpenGL.GL_TEXTURE_2D);
gl.BindTexture(OpenGL.GL_TEXTURE_2D, texture[1]); // Select Our Texture
gl.Begin(OpenGL.GL_TRIANGLE_STRIP); // Build Quad From A Triangle Strip
gl.TexCoord(0, 0); gl.Vertex(winRightPos, 0.0); // Top Right
gl.TexCoord(1, 0); gl.Vertex(winLeftPos, 0.0); // Top Left
gl.TexCoord(0, 1); gl.Vertex(winRightPos, 0.15 * (double)Height); // Bottom Right
gl.TexCoord(1, 1); gl.Vertex(winLeftPos, 0.15 * (double)Height); // Bottom Left
gl.End(); // Done Building Triangle Strip
//disable, straight color
gl.Disable(OpenGL.GL_TEXTURE_2D);
}
//LightBar if AB Line is set and turned on
if (isLightbarOn)
{
if (ct.isContourBtnOn)
{
txtDistanceOffABLine.Visible = true;
DrawLightBar(openGLControl.Width, openGLControl.Height, ct.distanceFromCurrentLine);
txtDistanceOffABLine.Text = " " + Convert.ToString((int)Math.Abs(ct.distanceFromCurrentLine * 0.3937)) + " ";
if (Math.Abs(ABLine.distanceFromCurrentLine) > 15.0)
{
txtDistanceOffABLine.ForeColor = Color.Yellow;
}
else
{
txtDistanceOffABLine.ForeColor = Color.LightGreen;
}
}
else
{
if (ABLine.isABLineSet | ABLine.isABLineBeingSet)
{
txtDistanceOffABLine.Visible = true;
DrawLightBar(openGLControl.Width, openGLControl.Height, ABLine.distanceFromCurrentLine);
txtDistanceOffABLine.Text = " " + Convert.ToString((int)Math.Abs(ABLine.distanceFromCurrentLine * 0.3937)) + " ";
if (Math.Abs(ABLine.distanceFromCurrentLine) > 15.0)
{
txtDistanceOffABLine.ForeColor = Color.Yellow;
}
else
{
txtDistanceOffABLine.ForeColor = Color.LightGreen;
}
}
}
//AB line is not set so turn off numbers
if (!ABLine.isABLineSet & !ABLine.isABLineBeingSet & !ct.isContourBtnOn)
{
txtDistanceOffABLine.Visible = false;
}
}
else
{
txtDistanceOffABLine.Visible = false;
}
gl.Flush(); //finish openGL commands
gl.PopMatrix(); // Pop the modelview.
// back to the projection and pop it, then back to the model view.
gl.MatrixMode(OpenGL.GL_PROJECTION);
gl.PopMatrix();
gl.MatrixMode(OpenGL.GL_MODELVIEW);
//reset point size
gl.PointSize(1.0f);
gl.Flush();
//draw the section control window off screen buffer
openGLControlBack.DoRender();
}
}
