[modules] nav_survey_poly_osam: sweep improvements

Added half U-turn even if not sweepback
Some modification to change sweep distance and number of points using telemetry parametrers
option to use default 1/2 sweep distance for start line or using alternative 1/32 sweep start

Its is very useful when using poligon survey osam for photogrametry

closes #938

conf/flight_plans/nav_modules.xml

@@ -3,17 +3,17 @@
 <flight_plan alt="260" ground_alt="185" lat0="43.46223" lon0="1.27289" max_dist_from_home="1500" name="Basic" security_height="25">
   <header>
 #include "subsystems/datalink/datalink.h"
-
 #include "modules/digital_cam/dc.h"
 #define LINE_START_FUNCTION dc_Survey(40);
 #define LINE_STOP_FUNCTION dc_autoshoot = DC_AUTOSHOOT_STOP;
-  </header>
+</header>
   <waypoints>
     <waypoint name="HOME" x="0" y="0"/>
     <waypoint name="STDBY" x="49.5" y="100.1"/>
     <waypoint name="1" x="10.1" y="189.9"/>
     <waypoint name="2" x="132.3" y="139.1"/>
     <waypoint name="MOB" x="137.0" y="-11.6"/>
+    <waypoint name="S1_angle" x="156.2" y="222.2"/>
     <waypoint name="S1" x="-109.0" y="245.9"/>
     <waypoint name="S2" x="274.4" y="209.5"/>
     <waypoint name="S3" x="322.1" y="29.9"/>
@@ -24,9 +24,8 @@
     <waypoint name="_BASELEG" x="168.8" y="-13.8"/>
     <waypoint name="CLIMB" x="-114.5" y="162.3"/>
   </waypoints>
-
   <sectors>
-    <sector name="POLY_SURVEY" color="green">
+    <sector color="green" name="POLY_SURVEY">
       <corner name="S1"/>
       <corner name="S2"/>
       <corner name="S3"/>
@@ -34,7 +33,6 @@
       <corner name="S5"/>
     </sector>
   </sectors>
-
   <exceptions/>
   <blocks>
     <block name="Wait GPS">
@@ -76,7 +74,6 @@
     <block group="extra_pattern" name="Survey S1-S3" strip_button="Survey (wp S1-S3)" strip_icon="survey.png">
       <survey_rectangle grid="150" wp1="S1" wp2="S3"/>
     </block>
-    <!--===================== Nav modules example blocks =================================-->
     <block name="Bungee take-off">
       <call fun="nav_bungee_takeoff_setup(WP_HOME)"/>
       <call fun="nav_bungee_takeoff_run()"/>
@@ -90,7 +87,7 @@
       <call fun="nav_line_border_run(WP_1, WP_2, nav_radius)"/>
     </block>
     <block group="nav_pattern" name="Smooth nav (wp 1-2)" strip_button="Smooth nav">
-      <set var="snav_desired_tow" value="gps.tow / 1000. + 60."/>
+      <set value="gps.tow / 1000. + 60." var="snav_desired_tow"/>
       <call fun="snav_init(WP_1, M_PI_2-atan2(WaypointY(WP_2)-WaypointY(WP_1),WaypointX(WP_2)-WaypointX(WP_1)), DEFAULT_CIRCLE_RADIUS/2.)"/>
       <call fun="snav_circle1()"/>
       <call fun="snav_route()"/>
@@ -104,7 +101,11 @@
       <call fun="nav_flower_run()"/>
     </block>
     <block name="Poly survey osam">
-      <call fun="nav_survey_poly_osam_setup(WP_S1, 5, 100, 45)"/>
+      <call fun="nav_survey_poly_osam_setup(WP_S1, 5, 100, -5)"/>
+      <call fun="nav_survey_poly_osam_run()"/>
+    </block>
+    <block name="Poly survey osam dynamic">
+      <call fun="nav_survey_poly_osam_setup_towards(WP_S1, 0, 0, WP_S1_angle)"/>
       <call fun="nav_survey_poly_osam_run()"/>
     </block>
     <block name="ZamboniSurvey">
@@ -122,8 +123,6 @@
       <call fun="nav_spiral_setup(WP_S1, WP_S2, 30, 10, 3)"/>
       <call fun="nav_spiral_run()"/>
     </block>
-    <!--====================================================================================-->
-
     <block group="land" name="Land Right AF-TD" strip_button="Land right (wp AF-TD)" strip_icon="land-right.png">
       <set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
       <deroute block="land"/>

conf/modules/nav_survey_poly_osam.xml

@@ -3,9 +3,28 @@
 <module name="nav_survey_poly" dir="nav">
   <doc>
     <description>
-       Polygon survey from OSAM
+Polygon survey from OSAM.
+You can use:
+* nav_survey_poly_osam_setup function for static definitions or
+* nav_survey_poly_osam_setup_towards for a more flexible setup
     </description>
+    <define name="POLY_OSAM_POLYGONSIZE" value="10" description="max waypoints usable in polygon survey"/>
+    <define name="POLY_OSAM_ENTRY_RADIUS" value="0" description="entry circle radius in meters. If 0 radius equal to half sweep"/>
+    <define name="POLY_OSAM_DEFAULT_SIZE" value="5" description="default number of waypoints/corners used in polygon survey. Can be changed by telemetry"/>
+    <define name="POLY_OSAM_DEFAULT_SWEEP" value="120" description="default distance in m between lines. Can be changed by telemetry"/>
+    <define name="POLY_OSAM_MIN_RADIUS" value="30" description="min radius used in U-turns"/>
+    <define name="POLY_OSAM_FIRST_SWEEP_DISTANCE" value="4" description="distante between the polygon border and first line.If 0 the distance well be equal to half sweep"/>
   </doc>
+
+  <settings>
+    <dl_settings>
+      <dl_settings NAME="P_Survey">
+        <dl_setting min="0" max="20" step="1" var="Poly_Size" type="uint8_t" shortname="Size" module="modules/nav/nav_survey_poly_osam"/>
+        <dl_setting min="0" max="500" step="1" var="Poly_Sweep" type="float" shortname="Sweep" module="modules/nav/nav_survey_poly_osam"/>
+      </dl_settings>
+    </dl_settings>
+  </settings>
+
   <header>
     <file name="nav_survey_poly_osam.h"/>
   </header>

sw/airborne/modules/nav/nav_survey_poly_osam.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2008-2013 The Paparazzi Team
+ * Copyright (C) 2008-2014 The Paparazzi Team
  *
  * This file is part of paparazzi.
  *
@@ -31,6 +31,41 @@
 #include "autopilot.h"
 #include "generated/flight_plan.h"
 
+#ifndef POLY_OSAM_DEFAULT_SIZE
+#define POLY_OSAM_DEFAULT_SIZE 5
+#endif
+
+#ifndef POLY_OSAM_DEFAULT_SWEEP
+#define POLY_OSAM_DEFAULT_SWEEP 100
+#endif
+
+/// Default entry radius, if 0 default to half sweep
+#ifndef POLY_OSAM_ENTRY_RADIUS
+#define POLY_OSAM_ENTRY_RADIUS 0
+#endif
+
+/// if 0 never check for min radius
+#ifndef POLY_OSAM_MIN_RADIUS
+#define POLY_OSAM_MIN_RADIUS 30
+#endif
+
+/// if 0 default to half sweep
+#ifndef POLY_OSAM_FIRST_SWEEP_DISTANCE
+#define POLY_OSAM_FIRST_SWEEP_DISTANCE 0
+#endif
+
+/// maximum number of polygon corners
+#ifndef POLY_OSAM_POLYGONSIZE
+#define POLY_OSAM_POLYGONSIZE 10
+#endif
+
+#ifndef POLY_OSAM_USE_FULL_CIRCLE
+#define POLY_OSAM_USE_FULL_CIRCLE TRUE
+#endif
+
+uint8_t Poly_Size = POLY_OSAM_DEFAULT_SIZE;
+float Poly_Sweep = POLY_OSAM_DEFAULT_SWEEP;
+bool_t use_full_circle = POLY_OSAM_USE_FULL_CIRCLE;
 
 bool_t nav_survey_poly_osam_setup_towards(uint8_t FirstWP, uint8_t Size, float Sweep, int SecondWP)
 {
@@ -38,10 +73,12 @@ bool_t nav_survey_poly_osam_setup_towards(uint8_t FirstWP, uint8_t Size, float S
   float dy = waypoints[SecondWP].y - waypoints[FirstWP].y;
   if (dx == 0.0f) dx = 0.000000001;
   float ang = atan(dy/dx);
+  //if values passed, use it.
+  if ( Size == 0 ) {Size = Poly_Size;}
+  if ( Sweep == 0) {Sweep = Poly_Sweep;}
   return nav_survey_poly_osam_setup(FirstWP, Size, Sweep, DegOfRad(ang));
 }
 
-
 struct Point2D {float x; float y;};
 struct Line {float m;float b;float x;};
 
@@ -50,7 +87,7 @@ static void RotateAndTranslateToWorld(struct Point2D *p, float Zrot, float trans
 static void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2);
 static float EvaluateLineForX(float y, struct Line L);
 
-#define PolygonSize 10
+#define PolygonSize POLY_OSAM_POLYGONSIZE
 #define MaxFloat   1000000000
 #define MinFloat   -1000000000
 
@@ -84,6 +121,7 @@ static float SurveyCircleQdr;
 static float MaxY;
 uint16_t PolySurveySweepNum;
 uint16_t PolySurveySweepBackNum;
+float EntryRadius;
 
 bool_t nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float Orientation)
 {
@@ -97,6 +135,20 @@ bool_t nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float
   float temp;
   float XIntercept1 = 0;
   float XIntercept2 = 0;
+  float entry_distance;
+
+  float PolySurveyEntryDistance = POLY_OSAM_FIRST_SWEEP_DISTANCE;
+  float PolySurveyEntryRadius = POLY_OSAM_ENTRY_RADIUS;
+
+  if (PolySurveyEntryDistance == 0)
+    entry_distance = sw/2;
+  else
+    entry_distance = PolySurveyEntryDistance;
+
+  if (PolySurveyEntryRadius == 0)
+    EntryRadius = sw/2;
+  else
+    EntryRadius = PolySurveyEntryRadius;
 
   SurveyTheta = RadOfDeg(Orientation);
   PolySurveySweepNum = 0;
@@ -220,9 +272,16 @@ bool_t nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float
 
     //Find amount to increment by every sweep
     if(EntryPoint.y >= MaxY/2)
+    {
+      entry_distance = -entry_distance;
+      EntryRadius = -EntryRadius;
       dSweep = -sw;
+    }
     else
+    {
+      EntryRadius = EntryRadius;
       dSweep = sw;
+    }
 
     //CircleQdr tells the plane when to exit the circle
     if(dSweep >= 0)
@@ -231,7 +290,7 @@ bool_t nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float
       SurveyCircleQdr = 180-DegOfRad(SurveyTheta);
 
     //Find y value of the first sweep
-    ys = EntryPoint.y+(dSweep/2);
+    ys = EntryPoint.y+entry_distance;
 
     //Find the edges which intercet the sweep line first
     for(i = 0; i < SurveySize; i++)
@@ -263,15 +322,15 @@ bool_t nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float
 
     //Find the direction to circle
     if(ys > 0 && SurveyToWP.x > SurveyFromWP.x)
-      SurveyRadius = dSweep/2;
+      SurveyRadius = EntryRadius;
     else if(ys < 0 && SurveyToWP.x < SurveyFromWP.x)
-      SurveyRadius = dSweep/2;
+      SurveyRadius = EntryRadius;
     else
-      SurveyRadius = -dSweep/2;
+      SurveyRadius = -EntryRadius;
 
     //Find the entry circle
     SurveyCircle.x = SurveyFromWP.x;
-    SurveyCircle.y = EntryPoint.y;
+    SurveyCircle.y = EntryPoint.y + entry_distance - EntryRadius;
 
     //Go into entry circle state
     CSurveyStatus = Entry;
@@ -289,11 +348,14 @@ bool_t nav_survey_poly_osam_run(void)
   float ys;
   static struct Point2D LastPoint;
   int i;
-  bool_t SweepingBack = FALSE;
+  bool_t LastHalfSweep;
+  static bool_t HalfSweep = FALSE;
   float XIntercept1 = 0;
   float XIntercept2 = 0;
   float DInt1 = 0;
   float DInt2 = 0;
+  float temp;
+  float min_radius = POLY_OSAM_MIN_RADIUS;
 
   NavVerticalAutoThrottleMode(0); /* No pitch */
   NavVerticalAltitudeMode(waypoints[SurveyEntryWP].a, 0.);
@@ -314,22 +376,38 @@ bool_t nav_survey_poly_osam_run(void)
     {
       CSurveyStatus = Sweep;
       nav_init_stage();
-      LINE_START_FUNCTION;
+      //LINE_START_FUNCTION;
     }
     break;
   case Sweep:
-    //Rotate and Translate Line points into real world
+    LastHalfSweep = HalfSweep;
     ToP.x = SurveyToWP.x;
     ToP.y = SurveyToWP.y;
     FromP.x = SurveyFromWP.x;
     FromP.y = SurveyFromWP.y;
 
+    //Rotate and Translate de plane position to local world
+    C.x = stateGetPositionEnu_f()->x;
+    C.y = stateGetPositionEnu_f()->y;
+    TranslateAndRotateFromWorld(&C, SurveyTheta, 0, 0);
+    TranslateAndRotateFromWorld(&C, 0, SmallestCorner.x, SmallestCorner.y);
+
+    //calc distance from line start and plane position (use only X position because y can be far due to wind or other factor)
+    float dist = FromP.x - C.x;
+
+    //Rotate and Translate Line points into real world
     RotateAndTranslateToWorld(&ToP, 0, SmallestCorner.x, SmallestCorner.y);
     RotateAndTranslateToWorld(&ToP, SurveyTheta, 0, 0);
 
     RotateAndTranslateToWorld(&FromP, 0, SmallestCorner.x, SmallestCorner.y);
     RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0);
 
+    // verify if plane are less than 10 meter from line start
+    if  ((dc_autoshoot == DC_AUTOSHOOT_STOP) &&  (fabs(dist) < 10))
+    {
+      LINE_START_FUNCTION;
+    }
+
     //follow the line
     nav_route_xy(FromP.x,FromP.y,ToP.x,ToP.y);
     if(nav_approaching_xy(ToP.x,ToP.y,FromP.x,FromP.y, 0))
@@ -337,21 +415,45 @@ bool_t nav_survey_poly_osam_run(void)
       LastPoint.x = SurveyToWP.x;
       LastPoint.y = SurveyToWP.y;
 
-      if(LastPoint.y+dSweep >= MaxY || LastPoint.y+dSweep <= 0) //Your out of the Polygon so Sweep Back
+      if(LastPoint.y+dSweep >= MaxY || LastPoint.y+dSweep <= 0) //Your out of the Polygon so Sweep Back or Half Sweep
+      {
+         if(LastPoint.y+(dSweep/2) >= MaxY || LastPoint.y+(dSweep/2) <= 0) //Sweep back
          {
           dSweep = -dSweep;
+          if (LastHalfSweep)
+          {
+            HalfSweep = FALSE;
+            ys = LastPoint.y+(dSweep);
+          }
+          else
+          {
+            HalfSweep = TRUE;
+            ys = LastPoint.y+(dSweep/2);
+          }
+
+          if(dSweep >= 0)
+            SurveyCircleQdr = -DegOfRad(SurveyTheta);
+          else
+            SurveyCircleQdr = 180-DegOfRad(SurveyTheta);
+          PolySurveySweepBackNum++;
+         }
+         else // Half Sweep forward
+         {
           ys = LastPoint.y+(dSweep/2);
 
           if(dSweep >= 0)
             SurveyCircleQdr = -DegOfRad(SurveyTheta);
           else
           SurveyCircleQdr = 180-DegOfRad(SurveyTheta);
-        SweepingBack = TRUE;
+          HalfSweep = TRUE;
-        PolySurveySweepBackNum++;
          }
-      else
+
+
+      }
+      else // Normal sweep
       {
         //Find y value of the first sweep
+        HalfSweep = FALSE;
         ys = LastPoint.y+dSweep;
       }
 
@@ -416,26 +518,34 @@ bool_t nav_survey_poly_osam_run(void)
         }
       }
 
+      //Find the radius to circle
+      if (!HalfSweep || use_full_circle)
+        temp = dSweep/2;
+      else
+        temp = dSweep/4;
 
+      //if less than min radius
+      if (fabs(temp) < min_radius)
+        {
+          if (temp < 0) temp = -min_radius; else temp = min_radius;
+        }
 
+      //Find the direction to circle
+      if(ys > 0 && SurveyToWP.x > SurveyFromWP.x)
+        SurveyRadius = temp;
+      else if(ys < 0 && SurveyToWP.x < SurveyFromWP.x)
+        SurveyRadius = temp;
+      else
+        SurveyRadius = -temp;
+
+      //find x position to circle
       if(fabs(LastPoint.x-SurveyToWP.x) > fabs(SurveyFromWP.x-SurveyToWP.x))
         SurveyCircle.x = LastPoint.x;
       else
         SurveyCircle.x = SurveyFromWP.x;
 
-
+      //y position to circle
-      if(!SweepingBack)
+      SurveyCircle.y = ys - temp;
-        SurveyCircle.y = LastPoint.y+(dSweep/2);
-      else
-        SurveyCircle.y = LastPoint.y;
-
-      //Find the direction to circle
-      if(ys > 0 && SurveyToWP.x > SurveyFromWP.x)
-        SurveyRadius = dSweep/2;
-      else if(ys < 0 && SurveyToWP.x < SurveyFromWP.x)
-        SurveyRadius = dSweep/2;
-      else
-        SurveyRadius = -dSweep/2;
 
       //Go into circle state
       CSurveyStatus = SweepCircle;
@@ -459,7 +569,7 @@ bool_t nav_survey_poly_osam_run(void)
     {
       CSurveyStatus = Sweep;
       nav_init_stage();
-      LINE_START_FUNCTION;
+      //LINE_START_FUNCTION;
     }
     break;
   case Init:

sw/airborne/modules/nav/nav_survey_poly_osam.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2008-2013 The Paparazzi Team
+ * Copyright (C) 2008-2014 The Paparazzi Team
  *
  * This file is part of paparazzi.
  *
@@ -29,6 +29,11 @@
 
 #include "std.h"
 
+extern uint8_t Poly_Size;
+extern float Poly_Sweep;
+extern uint16_t PolySurveySweepNum;
+extern uint16_t PolySurveySweepBackNum;
+
 /**
  * Setup polygon survey.
  * @param FirstWP      first waypoint/corner of the polygon
@@ -39,11 +44,14 @@
 extern bool_t nav_survey_poly_osam_setup(uint8_t FirstWP, uint8_t Size, float Sweep, float Orientation);
 
 /**
- * Setup polygon survey with sweep orientation towards a waypoint.
+ * Setup "dynamic" polygon survey with sweep orientation towards a waypoint.
  * Computes the sweep orientation angle from the line FirstWP-SecondWP.
+ * If you pass zero for Size and/or Sweep it will use the global Poly_Size and
+ * Poly_Sweep variables respectively (which can be changed via telemetry/settings).
  * @param FirstWp   first waypoint/corner of the polygon
- * @param Size      number of waypoints/corners used to define the polygon
+ * @param Size      number of waypoints/corners used to define the polygon,
- * @param Sweep     distance between scan lines
+ *                  if zero uses Poly_Size
+ * @param Sweep     distance between scan lines, if zero uses Poly_Sweep
  * @param SecondWp  second waypoint towards which the sweep orientation is computed
  */
 extern bool_t nav_survey_poly_osam_setup_towards(uint8_t FirstWP, uint8_t Size, float Sweep, int SecondWP);
@@ -51,7 +59,4 @@ extern bool_t nav_survey_poly_osam_setup_towards(uint8_t FirstWP, uint8_t Size,
 /** Run polygon survey */
 extern bool_t nav_survey_poly_osam_run(void);
 
-extern uint16_t PolySurveySweepNum;
-extern uint16_t PolySurveySweepBackNum;
-
 #endif