Difference between revisions of "Advanced Navigation Routines"

From PaparazziUAV
Jump to navigation Jump to search
Line 18: Line 18:


Then you can add flower to your flight plan:
Then you can add flower to your flight plan:
<br/>


   <block name="Flower">
   <block name="Flower">

Revision as of 13:41, 3 March 2011

OSAM Team Navigation Routines

Flower

Screen shot of flower routine

The flower navigation routine flies the aircraft in a flower pattern defined by two waypoints. The center waypoint defines the center of the flower and the altitude the plane flies at. The edge waypoint defines the radius of the flower.

In our example the waypoints are called "Center" and "Edge":

 <waypoint name="Center" x="-20.0" y="-160.0"/>
 <waypoint name="Edge" x="-10.0" y="-60.0"/>

To use this navigation routine, you need the extra navigation subsystem in your airframe file:

 <subsystem name="navigation" type="extra"/>

To avoid warnings while building you should include OSAMNav.h in your flight plan:

 #include "subsystems/navigation/OSAMNav.h"

Then you can add flower to your flight plan:

 <block name="Flower">
   <call fun="InitializeFlower(WP_Center,WP_Edge)"/>
   <call fun="FlowerNav()"/>
 </block>

Note that in the function InitializeFlower the waypoints need the prefix "WP_".

Bungee Takeoff

The bungee takeoff routine helps to automate takeoff by turning the throttle on after the bungee has been release from the hook. The only waypoint you need for this routine is the position of where the bungee is pegged to the ground. Using this waypoint, a line is drawn from the position of the aircraft to the bungee waypoint. This line is called the launch line and will stop updating once the speed of the plane exceeds the MinSpeed. This allows the user to initialize the routine, move the plane and launch it without having to reinitialize. Once the plane is launched, it will follow the launch line until it crosses the throttle line. The throttle line is a line perpendicular to the launch line at a distance d from the bungee waypoint (see the diagram below). When the plane crosses the throttle line and the speed of the aircraft is greater than MinSpeed, the throttle comes on. After the throttle comes on, the plane keeps going straight until it reaches a specified speed and altitude above the bungee waypoint altitude. When it reaches the takeoff speed and takeoff altitude, the next block in the flight plan is executed. The takeoff speed, takeoff altitude, MinSpeed and the distance d from the bungee waypoint are specified in the airframe file. You will need to add those values to your airframe file like this...

 <section name="Takeoff" prefix="Takeoff_">
   <define name="Height" value="30" unit="m"/>
   <define name="Speed" value="15" unit="m/s"/>
   <define name="Distance" value="3" unit="m"/>
   <define name="MinSpeed" value="5" unit="m/s"/>    
 </section>

To use this navigation routine, include OSAMNav.h in your flight plan and OSAMNav.c to your airframe file. Then you can add the bungee takeoff routine to your flight plan like so...

 <block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
   <call fun="InitializeBungeeTakeoff(WP_Bungee)"/>
   <call fun="BungeeTakeoff()"/>
 </block>

To get this routine to work consistantly, you will need to tune the values in the airframe config file. If the prop doesn't automatically turn on when it crosses the throttle line, it could be because the Distance and/or the MinSpeed are too big. If it turns on to early, it could be because the Distance and/or MinSpeed are too small.

***Precaution should be taken while tuning the auto takeoff. If the MinSpeed is too low, the prop could turn on while holding the aircraft!***

Polygon Survey

Explanation

With this navigation routine, an aircraft can survey the area of any convex polygon given an entry point, the number of waypoints which define the polygon, the sweep width and the desired orientation of the sweeps.

The entry point is the first corner of the polygon and the point at which the aircraft will begin surveying the area. When in the entry state, the aircraft will circle around the entry point in order to smoothly transition into the first sweep. The aircraft will also keep circling around the entry point until it gets to the waypoint altitude. After the first sweep is made, the direction of the next sweep is determined by the distance of the entry point to the edges of the polygon. If there is more area above the first sweep, the aircraft will sweep up. If there is more area below the first sweep, the aircraft will sweep down.

The aircraft will keep sweeping back and forth until it reaches the end of the polygon. At this point, the aircraft will sweep back up/down the polygon halfway in between the sweeps previously made by the aircraft (just like the rectangle survey function). The aircraft will keep sweeping up and down the polygon unless the user manually exits the block or unless an exception is used (see below).

The orientation of the sweeps can ranges from north south to east west and any where in between (0-90 degrees respectively). The side of the polygon the aircraft starts on (ex. north or south) is determined by the side of the polygon the entry point is located.

Implementation

To use this navigation routine, you need to include OSAMNav.h in your flight plan and OSAMNav.c to your airframe file. Then add this navigation routine in your flight plan like so...

   <block name="Poly Survey">
     <call fun="InitializePolygonSurvey(WP_S1, NumOfCorners, SweepWidth, Orientation)"/>
     <call fun="PolygonSurvey()"/>
   </block>

The parameters are the entry waypoint, the number of waypoints in the polygon, the sweep width (meters), and the desired orientation of the sweeps (degrees). The maximum number of waypoints a polygon can have is currently ten (can be changed in the code). If the number of waypoints in the polygon exceeds the maximum number, the routine will exit and move to the next block in the flight plan. The routine will also exit if the orientation is not between 0 and 90 degrees.

Here is an example of how you should declare each of the corners of the polygon.

  <waypoint alt="1453.0" name="S1" x="-546.2" y="297.4"/>
  <waypoint alt="1453.0" name="_S2" x="-129.8" y="744.1"/>
  <waypoint alt="1553.0" name="_S3" x="1030.5" y="535.5"/>
  <waypoint alt="1453.0" name="_S4" x="523.0" y="-236.7"/>
  <waypoint alt="1453.0" name="_S5" x="-285.9" y="-255.7"/>

S1 is the entry waypoint and the first corner. The other corners should be in order clockwise or counter clockwise around the polygon. Even though this group of waypoints must be declared together, where the group appears in the list of waypoints doesn't matter.

If you want the edges of the polygon to show up on the GCS, you can also declare the polygon as a sector. This is not required to run the routine.

 <sectors>
   <sector name="PolySector">
     <corner name="S1"/>
     <corner name="_S2"/>
     <corner name="_S3"/>
     <corner name="_S4"/>
     <corner name="_S5"/>
   </sector>
 </sectors>

Alternative configurations

If you are wanting to start and stop the Poly Survey this is possible by splitting the Poly Survey code above into the initialization routine and the execution routine. You might want to do this if you are searching for something inside the Poly Survey, think you have found it and then realized you haven't, so you want to continue the survey (not restart it). Using the example above, but with the initialisation and execution code separate:

   <block name="Init Poly Survey">
     <call fun="InitializePolygonSurvey(WP_S1, NumOfCorners, SweepWidth, Orientation)"/>
   </block>
   <block name="Execute Poly Survey">
     <call fun="PolygonSurvey()"/>
   </block>

Exceptions

There are a couple of built in variables which can be used to exit the routine with an exception. PolySurveySweepNum gives the number of sweeps the aircraft has made and PolySurveySweepBackNum gives the number of times the aircraft has gotten to the bottom of the polygon and swept back. The first example would deroute the aircraft to standby after the aircraft made it's second sweep. The second example would deroute the aircraft to standby before it starts to sweep back up the polygon for the first time.

   <block name="Poly Survey">
      <exception cond="PolySurveySweepNum >= 2" deroute="Standby"/>
     <call fun="InitializePolygonSurvey(WP_S1, 5, 200, 45)"/>
     <call fun="PolygonSurvey()"/>
   </block>
   <block name="Poly Survey">
      <exception cond="PolySurveySweepBackNum >= 1" deroute="Standby"/>
     <call fun="InitializePolygonSurvey(WP_S1, 5, 200, 45)"/>
     <call fun="PolygonSurvey()"/>
   </block>

Flight Line

The Flight Line Routine allows the user to map/follow one dimensional areas of interest like roads and rivers. Given two waypoints, the routine will automatically transition into the flight line to ensure full coverage between the waypoints. In addition, distances before and after the flight line can be used to add extra security to the coverage.

Flight Line Diagram

To use this navigation routine, you need to include OSAMNav.h in your flight plan and OSAMNav.c to your airframe file. Then add this navigation routine in your flight plan like so...

   <block name="Map River">
     <call fun="FlightLine(WP_R1,WP_R2,nav_radius,distance_before,distance_after)"/>
   </block>

Multiple flight lines can be daisy chained by reusing waypoints as shown below...

   <block name="Map River">
     <call fun="FlightLine(WP_R1,WP_R2,nav_radius,100,100)"/>
     <call fun="FlightLine(WP_R2,WP_R3,nav_radius,100,100)"/>
     <call fun="FlightLine(WP_R3,WP_R4,nav_radius,100,100)"/>
     <call fun="FlightLine(WP_R5,WP_R6,nav_radius,100,100)"/>
     <deroute block="Standby"/>
   </block>

However, the previous block can also be implemented using the FlightLineBlock function. The FlightLineBlock function works the same as the FlightLine function except it automatically steps through the waypoints between the given waypoints. Make sure the waypoints are declared in order or else it won't work!

  <block name="Map River">
     <call fun="FlightLineBlock(WP_R1,WP_R6,nav_radius,100,100)"/>
     <deroute block="Standby"/>
   </block>