AgOpenGPS - Page 11 - The Combine Forum
 2070Likes
 
LinkBack Thread Tools
post #101 of 3975 (permalink) Old 11-02-2016, 08:44 PM
Senior Member
 
Join Date: Sep 2009
Location: S. AB
Posts: 3,469
Mentioned: 6 Post(s)
Quoted: 861 Post(s)
Yes I think all I need to do is provide you with the right implement heading.

Coming from the Linux world, I find Windows development a bit arcane. But I just found a way to make my life easier. If I change the application type to "Console" when I run it it pops up a DOS window that I can write to from anywhere in the program using System.Console.WriteLine. Should help with my debugging.

torriem is offline  
Sponsored Links
Advertisement
 
post #102 of 3975 (permalink) Old 11-02-2016, 10:11 PM
Senior Member
 
Join Date: Sep 2009
Location: S. AB
Posts: 3,469
Mentioned: 6 Post(s)
Quoted: 861 Post(s)
Success I think. Still needs some more testing to make sure there are no corner cases I haven't dealt with yet. I still need a saner way to begin when the fix is uncertain and we aren't moving yet. I should probably fork your github repo and then give you a pull request. But for now I think I'll just give you a diff shortly.

I also decided to use the actual heading from the NMEA messages for both the camera view and the tractor's heading. Neither of these figures are essential for accurate operation once the section heading is worked out. I was very pleased with the results. The Deere signal gives a fairly steady heading which makes the view and the tractor stop wiggling around, making me dizzy! Though of course starting out from a dead stop it still does all kinds of weird things until it knows the heading, but the calculated heading does that too.

By the way i think part of my wavy GPS readings stem from the fact that as the tractor is driving a straight line on auto steer, since it's an articulated tractor, the nose is constantly waving back and forth a very slight amount and the GPS picks up on that since it's tracking at 5 times a second. This can be tuned in the StarFire system, but it seems like the nature of the beast with articulated tractors. That doesn't explain the waviness when I'm driving myself without moving the steering wheel, but I think pitch and yaw could be contributing to that. I don't think the NMEA output is compensated on GreenStar receivers. We're just on SF1 too.

Attached Images
File Type: png turncomp1.png (60.5 KB, 31 views)

Last edited by torriem; 11-02-2016 at 10:56 PM.
torriem is offline  
post #103 of 3975 (permalink) Old 11-02-2016, 10:50 PM Thread Starter
Senior Member
 
Join Date: Aug 2012
Location: Vermilion Alberta Canada
Posts: 5,599
Mentioned: 11 Post(s)
Quoted: 2456 Post(s)
That looks great! Or do you just want to throw the code up on here in a block?
BrianTee is offline  
Sponsored Links
Advertisement
 
post #104 of 3975 (permalink) Old 11-02-2016, 11:39 PM
Senior Member
 
Join Date: Sep 2009
Location: S. AB
Posts: 3,469
Mentioned: 6 Post(s)
Quoted: 861 Post(s)
Since the code adds some member variables and is a bit long at present (a lot of commented-out code), can I just link to the change? Also there's a pull request on github that you might be able to get working.

https://github.com/torriem/AgOpenGPS...7f50bb92f1426d (click on view the diff)

And also the patch that uses NMEA headings is here:
https://github.com/torriem/AgOpenGPS...1622d83337dfbb

You can see I have several commented-out lines of System.Console.Write() that I used in debugging. Was very handy. I made my fork a Console App to enable this. Very handy.

By the way, feel free to use my patches in any way you see fit; I disclaim copyright over them so you could relicense and sell it if you want.

I've attached another picture of it working (though for debugging purposes I fixed the camera to North so the path was easier to tell if it was working right... we might want an option to fix the camera direction, especially in 2d mode). The booms turned on a bit early in this picture. I suspect it's just the 2 second look-ahead that did it.

EDIT. Here's the code for posterity:
Code:
                    if (vehicle.isHitched)
                    {

                        if (prevImplementNorthing == 0 && prevImplementEasting == 0) {
                            //No previous location known; assume it's straight back from the tractor
                            fixHeadingSection = fixHeading;
                            prevImplementEasting = pn.easting - Math.Sin(fixHeadingSection) * vehicle.toolForeAft;
                            prevImplementNorthing = pn.northing - Math.Cos(fixHeadingSection) * vehicle.toolForeAft;
                            //System.Console.WriteLine("Initialized to starting position.");
                            
                        } else {
                            if (Math.Abs(pn.northing - prevImplementNorthing) < 0.1 &&
                                Math.Abs(pn.easting - prevImplementEasting) < 0.1) {
                                // The new tractor position is exactly the same as the old
                                // implement position, so push the implement straight back.
                                fixHeadingSection = prevFixHeadingSection;
                                prevImplementEasting = pn.easting - Math.Sin(fixHeadingSection) * vehicle.toolForeAft;
                                prevImplementNorthing = pn.northing - Math.Cos(fixHeadingSection) * vehicle.toolForeAft;
                                //System.Console.WriteLine("Tractor moved to where implement was; pushing implement back.");
                            } else {

                                // Because this approximation over-estimates, break the change in tractor
                                // position into several sub-steps and do this approximation for each
                                // step.

                                double in_temp = prevImplementNorthing;
                                double ie_temp = prevImplementEasting;
                                double n_step = (pn.northing - prevNorthing[1]) / TURN_STEPS;
                                double e_step = (pn.easting - prevEasting[1]) / TURN_STEPS;
                                double tn_temp = prevNorthing[1];
                                double te_temp = prevEasting[1];

                                double t;

                                for (int i = 0; i < TURN_STEPS; i++) {
                                    //Break the tractor movement up into little steps and move
                                    //apply the approximation to each step.
                                    tn_temp += n_step;
                                    te_temp += e_step;
                                    t = vehicle.toolForeAft / Math.Sqrt( (tn_temp - in_temp) * (tn_temp - in_temp) +
                                                                         (te_temp - ie_temp) * (te_temp - ie_temp) );
                                    in_temp = tn_temp + t * (tn_temp - in_temp);
                                    ie_temp = te_temp + t * (te_temp - ie_temp);
                                }

                                //Now we are in the new position and the last approximation is the 
                                //new position of the implement.
                                prevImplementNorthing = in_temp;
                                prevImplementEasting = ie_temp;
                                
                                /* Uncomment for faster, but less precise calculation. Also it's less obfuscated.

                                // Draw a line as it were from the new tractor position to the old implement position
                                // measure the hitch length along that line and that becomes the new implement position.
                                // It over-estimates the sideways movement a bit, but with 5 samples per second it should
                                // approximate the actual position fairly well.  If we need more precision, we can do
                                // sub calculations in here.
                                double t = vehicle.toolForeAft / Math.Sqrt( (pn.northing - prevImplementNorthing) * (pn.northing - prevImplementNorthing) +
                                                                            (pn.easting - prevImplementEasting) * (pn.easting - prevImplementEasting));
                                prevImplementNorthing = pn.northing + t * (pn.northing - prevImplementNorthing);
                                prevImplementEasting = pn.easting + t * (pn.easting - prevImplementEasting);
                                */

                                fixHeadingSection = Math.Atan2(pn.easting - prevImplementEasting, pn.northing - prevImplementNorthing);
                            }
                        }
                        /*
                        System.Console.Write("Tractor heading is ");
                        System.Console.Write((fixHeading * 180.0 / Math.PI + 360) % 360);
                        System.Console.Write(" and implement heading is ");
                        System.Console.WriteLine((fixHeadingSection * 180.0 / Math.PI + 360) % 360);
                        */

                        prevFixHeadingSection = fixHeadingSection;


                        /*
                        double eastAvg = 0; 
                        double northAvg = 0;
                        
                        for (int d = 1; d < (int)(deltaTurn+1); d++)
                        {
                            eastAvg += prevEasting[d] / (double)deltaTurn;
                            northAvg += prevNorthing[d] / (double)deltaTurn;
                        }

                        fixHeadingSection = Math.Atan2(pn.easting - eastAvg, pn.northing - northAvg);
                        if (fixHeadingSection < 0) fixHeadingSection += Math.PI * 2.0;
            */
                    }
                    else fixHeadingSection = fixHeading;
Attached Images
File Type: png turncomp2.png (64.6 KB, 24 views)

Last edited by torriem; 11-03-2016 at 12:24 AM.
torriem is offline  
post #105 of 3975 (permalink) Old 11-03-2016, 01:51 AM Thread Starter
Senior Member
 
Join Date: Aug 2012
Location: Vermilion Alberta Canada
Posts: 5,599
Mentioned: 11 Post(s)
Quoted: 2456 Post(s)
Holy cow that works well!

Reduced the whole thing to this:

Code:
               //in radians
                    if (vehicle.isHitched)
                    {
                        //Torriem rules!!!!! Oh yes, this is all his. Thank-you
                        double t;
                        double dist = Math.Sqrt( (pn.northing - prevImplementNorthing) * (pn.northing - prevImplementNorthing) +
                                                                    (pn.easting - prevImplementEasting) * (pn.easting - prevImplementEasting));

                        if (dist != 0)
                        {
                            t = vehicle.toolForeAft / dist;
                            prevImplementNorthing = pn.northing + t * (pn.northing - prevImplementNorthing);
                            prevImplementEasting = pn.easting + t * (pn.easting - prevImplementEasting);
                            fixHeadingSection = Math.Atan2(pn.easting - prevImplementEasting, pn.northing - prevImplementNorthing);
                        }
                        prevFixHeadingSection = fixHeadingSection;

                    }
There is a block called " isFirstFixPositionSet" . In there i set prevImplement variables to current fix position. No far implement swing at startup, starts clean.

I'll throw on Github

Last edited by BrianTee; 11-03-2016 at 02:50 AM.
BrianTee is offline  
post #106 of 3975 (permalink) Old 11-03-2016, 09:39 AM
Senior Member
 
Join Date: Sep 2009
Location: S. AB
Posts: 3,469
Mentioned: 6 Post(s)
Quoted: 861 Post(s)
I think if dist is == 0, although the fixHeadingSection won't change, we still need to recompute prevImplement position for the next go-around; it would affect the next computation a little bit if prevImplement didn't move.

I am pretty sure the algorithm would deal with reversing well, but I want to test that just to see what happens.

Also it looks like prevFixHeadingSection is redundant. Is there also a reason for prevSectionNorthing and prevSectionEasting? I didn't notice those when I first created my code. Looks like I should have used those variables instead. Nevermind I see that those are for drawing.

Last edited by torriem; 11-03-2016 at 10:23 AM.
torriem is offline  
post #107 of 3975 (permalink) Old 11-03-2016, 09:58 AM
Senior Member
 
Join Date: Sep 2009
Location: S. AB
Posts: 3,469
Mentioned: 6 Post(s)
Quoted: 861 Post(s)
What did you think of using NMEA heading for camera and tractor heading after section heading is computed? It worked very well for me, and typically the NMEA heading is well-filtered in the receiver.

Code:
                    else fixHeadingSection = fixHeading;

                    //use NMEA headings for camera and tractor graphic
                    fixHeadingCam = pn.headingTrue;
                    fixHeading = pn.headingTrue * Math.PI / 180.0;
You mentioned trying it before but you had the tractor spin around. I don't get that behavior here.
torriem is offline  
post #108 of 3975 (permalink) Old 11-03-2016, 10:23 AM Thread Starter
Senior Member
 
Join Date: Aug 2012
Location: Vermilion Alberta Canada
Posts: 5,599
Mentioned: 11 Post(s)
Quoted: 2456 Post(s)
Quote:
Originally Posted by torriem View Post
What did you think of using NMEA heading for camera and tractor heading after section heading is computed? It worked very well for me, and typically the NMEA heading is well-filtered in the receiver.

Code:
                    else fixHeadingSection = fixHeading;

                    //use NMEA headings for camera and tractor graphic
                    fixHeadingCam = pn.headingTrue;
                    fixHeading = pn.headingTrue * Math.PI / 180.0;
You mentioned trying it before but you had the tractor spin around. I don't get that behavior here.
It seems to be very dependent on the unit. Outback outputs a very crappy heading in the NMEA - even with RTK. Its basically unuseable. Perhaps the ability to switch and use either in settings?

Also, i noticed the delay was set to almost nothing in these lines....

//array index for previous northing and easting positions
private static int numPrevs = 8;

fixHeading = Math.Atan2(pn.easting - prevEasting[1], pn.northing - prevNorthing[1]);
.....
fixHeadingCam = Math.Atan2(pn.easting - prevEasting[1], pn.northing - prevNorthing[1]);


If you change the prevEasting[1] to something like prevEasting[4] or even 6, that takes the fix from about a second ago (5 is one second) and then does the heading on a much longer line. The higher the number the less the wobble. Change to this.....

Code:
//array index for previous northing and easting positions
        private static int numPrevs = 20;
 
                    fixHeading = Math.Atan2(pn.easting - prevEasting[2], pn.northing - prevNorthing[2]);
                          .....
                    fixHeadingCam = Math.Atan2(pn.easting - prevEasting[17], pn.northing - prevNorthing[17]);
Selectable in settings would be ideal. Time to put something in that blank Display settings page.

Last edited by BrianTee; 11-03-2016 at 10:33 AM.
BrianTee is offline  
post #109 of 3975 (permalink) Old 11-03-2016, 10:37 AM Thread Starter
Senior Member
 
Join Date: Aug 2012
Location: Vermilion Alberta Canada
Posts: 5,599
Mentioned: 11 Post(s)
Quoted: 2456 Post(s)
Quote:
Originally Posted by torriem View Post
[/code]You mentioned trying it before but you had the tractor spin around. I don't get that behavior here.
It never spun around, it just wiggled a lot. Like what you are seeing with fixes. Its good to try lots of different ones. Do you have another GPS unit to try it on?

Try changing the number of previous in the array. It makes a huge difference.
BrianTee is offline  
post #110 of 3975 (permalink) Old 11-03-2016, 10:49 AM
Senior Member
 
Join Date: Sep 2009
Location: S. AB
Posts: 3,469
Mentioned: 6 Post(s)
Quoted: 861 Post(s)
All I have are John Deere StarFire receivers. I'll try it again with the delay (filtering/averaging). It would be best to have a general solution that would work across receivers with the minimal amount of user configuration.

BrianTee likes this.
torriem is offline  
Sponsored Links
Advertisement
 
Reply

Quick Reply
Message:
Options

Register Now



In order to be able to post messages on the The Combine Forum forums, you must first register.
Please enter your desired user name, your email address and other required details in the form below.

User Name:
Password
Please enter a password for your user account. Note that passwords are case-sensitive.

Password:


Confirm Password:
Email Address
Please enter a valid email address for yourself.

Email Address:
OR

Log-in










Thread Tools
Show Printable Version Show Printable Version
Email this Page Email this Page



Posting Rules  
You may post new threads
You may post replies
You may not post attachments
You may not edit your posts

BB code is On
Smilies are On
[IMG] code is On
HTML code is Off
Trackbacks are On
Pingbacks are On
Refbacks are On

 
For the best viewing experience please update your browser to Google Chrome