I read navigation.cpp from AP_Navigation library. I need to measure crosstrack of ArduRover measuring the current location (from GPS) with the desired point or path. Inside the code there is a crosstrack calculation. Somehow, I am not really understand what the code actually doing.
Can someone explain to me the basic theory behind the crosstrack code in AP_Navigation.h ? I still a beginner in this DIY drone.
Your explanation is much appreciated! Thanks...
Basic idea, draw a line from one way point to another.
Cross track error is how far to the right or left of this line are you?
My approach is to adjust the origin so the destination is at 0,0
Then draw a line from the start to this point.
Find the Normal to this line (IE rotate it 90 degrees is one simple way realize that sin/cos of 90 deg is 0,1 so your really just swaping x,y values)
Then given the slope of this line find the intersection of this line moved to the current position and the origional line.
The distance from here to that intersection is the cross track error.
I also compute the current heading to the destination and when that is more than 90 degrees from the origional heading I declare that waypoint passed and move on to the next.
This may not be what the APM code is doing, but it is what my personal AP code does.
Thanks for your explanation. So basically in my code what I really need to do to measure the lateral crosstrack:
1. Create a path y=mx + c where this is my reference path or straight line.
2. Get the intersection point. But I am not really clear how do I get the intersection ? For example, my current heading is 30 degree. So, to get the normal I just added with 90 degree. Thus, the normal slope is 120 degree heading. Then how do I calculate intersection point ? Any idea?
3.Then to get the lateral crosstrack distance I simply using rotational matrix ;
lateral crosstrack distance = − sin θ (Xreference − X current ) + cos θ (Yreference − Y current ) where θ is the heading direction.
But I am still not clear on step no 2...
A rover doesn't have crosstrack to deal with. Crosstrack is when your vehicle is also traveling perpendicular (or at some angle) to the direction it is heading. A plane or copter in a crosswind or a boat crossing a river would have to deal with crosstrack error.
How about if the current position of the rover is slightly different with our reference position. Are we calling it crosstrack error? or do we have any correct name. I am not too sure.
DId you look at http://code.google.com/p/ardupilot-mega/wiki/CrossTrack and
Here's another place to look: