Posted by Sami Finnila on April 16, 2009 at 5:06am
I'm not sure whether ArduPilot has line hold between waypoints but the problems in the SparkFun competition reminded me of something I recently programmed for my NXT AutoPilot. A true line hold algorithm in which the course is set fully proportionally to the distance from the desired line: i.e. it's not a 'carrot' based system like in paparazzi and it's therefore likely to give better results. It's been tested in X-Plane and it works beautifully. It takes in gps lat and lon and 2 waypoints and outputs the desired heading (rightHeading). Please note that I've used longtitudes as x-axis in my code!!The source in C is here://#define LineHoldCoefficient 900dX=(wayPoints[waypoint][1]-wayPoints[waypoint-1][1]);//deltaXdY=(wayPoints[waypoint][0]-wayPoints[waypoint-1][0]);//deltaYif(dX!=0) //Let's calculate the 'heading' of the desired line...{tempHeading=radiansToDegrees(atan(dY/dX));if(dX<0){if(dY<=0)tempHeading+=180;else if(tempHeading>270)tempHeading-=180;}}else if(dY!=0){if(dY<0)tempHeading=270;else tempHeading=90;}//Distance from the desired line.. k=dY/dX=-a/b//-dY*x + dX*y + 0 =0 (ax+by+c=0)..d=(abs(-dY*(gpsLon-wayPoints[waypoint-1][1])+dX*(gpsLat-wayPoints[waypoint-1][0]))) / (sqrt(dY*dY+dX*dX));//We need to choose which one of the prependicular directions to use as correction.//If our coordinates (gpsLon,gpsLat) are 'above' the desired line we need to go down and the other way round..float k=dY/dX;if(dX){float PrependicularDirection_down=0,PrependicularDirection_up=0;if((((int)tempHeading-90+360)%360)>180){PrependicularDirection_down=tempHeading-90;PrependicularDirection_up=tempHeading+90;}else{PrependicularDirection_down=tempHeading+90;PrependicularDirection_up=tempHeading-90;}if((k*(gpsLon-wayPoints[waypoint-1][1]))<(gpsLat-wayPoints[waypoint-1][0]))tempHeading=(tempHeading+LineHoldCoefficient*d*PrependicularDirection_down)/(1+LineHoldCoefficient*d);else tempHeading=(tempHeading+LineHoldCoefficient*d*PrependicularDirection_up)/(1+LineHoldCoefficient*d);}else //We're on the y-axis..{if((gpsLon-wayPoints[waypoint-1][1])>=0)tempHeading=(tempHeading+180*d*LineHoldCoefficient)/(1+LineHoldCoefficient*d);else tempHeading=(tempHeading)/(1+LineHoldCoefficient*d);}rightHeading=((int)tempHeading+360)%360;//Let's make sure rightHeading is 0-360..
You need to be a member of diydrones to add comments!
I haven't had the chance to fly during the last couple of weeks due to bad wheater.. But while spending my time inside I've in turn made some progress with the source code. I added a pitch fix based on GPS yaw data. Now I'm supposed to be able to make much steeper turns. I also perfected the PID loops and made some major changes to the code and added functions like the line hold.
I'm planning to do some heavy test flying this weekend if the wind calms down and hopefully it all goes well and I can finally publish the autopilot for real.
As a side note I would recommend anyone working on a IMU to go for 6 DOF straight away as 5 DOF doesn't seem to give you the performance you would get with a much cheaper IR stabilizer; it remains to be seen how well the GPS yaw replacement performs.
Thanks, Sami. That looks very interesting and I've called it to Jordi's attention.
How is the NXT autopilot going? Flown it yet? I'm keen to restart my project with commercially available hardware and would love to start with your code.
Replies
can you send me the theory to understund how you did the math for this?
thanks...
Earl
I'm planning to do some heavy test flying this weekend if the wind calms down and hopefully it all goes well and I can finally publish the autopilot for real.
As a side note I would recommend anyone working on a IMU to go for 6 DOF straight away as 5 DOF doesn't seem to give you the performance you would get with a much cheaper IR stabilizer; it remains to be seen how well the GPS yaw replacement performs.
How is the NXT autopilot going? Flown it yet? I'm keen to restart my project with commercially available hardware and would love to start with your code.