diff --git a/conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml b/conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml
index 89504749f5..5fb5a0fc97 100644
--- a/conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml
@@ -34,10 +34,12 @@ RP3 Lisa MX
     </module>
 
     <define name="AGR_CLIMB" />
+    <define name="POLY_OSAM_HALF_SWEEP_ENABLED" value="FALSE"/>
   </firmware>
 
   <modules>
     <module name="nav" type="line"/>
+    <module name="nav" type="survey_poly_osam"/>
     <module name="sys_mon"/>
   </modules>
 
diff --git a/conf/modules/nav_survey_poly_osam.xml b/conf/modules/nav_survey_poly_osam.xml
index 9647474bbc..be10b37bfc 100644
--- a/conf/modules/nav_survey_poly_osam.xml
+++ b/conf/modules/nav_survey_poly_osam.xml
@@ -21,6 +21,8 @@ You can use:
       <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_setting min="0" max="1" step="1" values="FALSE|TRUE" var="Half_Sweep_Enabled" type="bool" shortname="Half Sweep" module="modules/nav/nav_survey_poly_osam"/>
+        <dl_setting min="0" max="1" step="1" values="FALSE|TRUE" var="Reset_Sweep" type="bool" shortname="Reset Sweep" module="modules/nav/nav_survey_poly_osam" handler="ResetSweepNumber"/>
       </dl_settings>
     </dl_settings>
   </settings>
diff --git a/sw/airborne/modules/nav/nav_survey_poly_osam.c b/sw/airborne/modules/nav/nav_survey_poly_osam.c
index d8aa839ed1..3f00016996 100644
--- a/sw/airborne/modules/nav/nav_survey_poly_osam.c
+++ b/sw/airborne/modules/nav/nav_survey_poly_osam.c
@@ -67,9 +67,16 @@
 #define POLY_OSAM_USE_FULL_CIRCLE TRUE
 #endif
 
+// use half sweep at the end of polygon
+#ifndef POLY_OSAM_HALF_SWEEP_ENABLED
+#define POLY_OSAM_HALF_SWEEP_ENABLED TRUE
+#endif
+
 uint8_t Poly_Size = POLY_OSAM_DEFAULT_SIZE;
 float Poly_Sweep = POLY_OSAM_DEFAULT_SWEEP;
 bool use_full_circle = POLY_OSAM_USE_FULL_CIRCLE;
+bool Half_Sweep_Enabled = POLY_OSAM_HALF_SWEEP_ENABLED;
+bool Reset_Sweep = FALSE;
 
 bool nav_survey_poly_osam_setup_towards(uint8_t FirstWP, uint8_t Size, float Sweep, int SecondWP)
 {
@@ -91,6 +98,14 @@ 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);
 
+void nav_survey_poly_osam_ResetSweepNumber(bool rst)
+{
+  if (rst) {
+    PolySurveySweepBackNum = 0;
+    Reset_Sweep = FALSE;
+  }
+}
+
 #define PolygonSize POLY_OSAM_POLYGONSIZE
 #define MaxFloat   1000000000
 #define MinFloat   -1000000000
@@ -409,7 +424,8 @@ bool nav_survey_poly_osam_run(void)
         LastPoint.y = SurveyToWP.y;
 
         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
+
+          if (LastPoint.y + (dSweep / 2) >= MaxY || LastPoint.y + (dSweep / 2) <= 0 || !Half_Sweep_Enabled) { //Sweep back
             dSweep = -dSweep;
             if (LastHalfSweep) {
               HalfSweep = false;
diff --git a/sw/airborne/modules/nav/nav_survey_poly_osam.h b/sw/airborne/modules/nav/nav_survey_poly_osam.h
index 8f00714d2c..9f3389f94f 100644
--- a/sw/airborne/modules/nav/nav_survey_poly_osam.h
+++ b/sw/airborne/modules/nav/nav_survey_poly_osam.h
@@ -33,6 +33,8 @@ extern uint8_t Poly_Size;
 extern float Poly_Sweep;
 extern uint16_t PolySurveySweepNum;
 extern uint16_t PolySurveySweepBackNum;
+extern bool Half_Sweep_Enabled;
+extern bool Reset_Sweep;
 
 /**
  * Setup polygon survey.
@@ -59,4 +61,6 @@ extern bool nav_survey_poly_osam_setup_towards(uint8_t FirstWP, uint8_t Size, fl
 /** Run polygon survey */
 extern bool nav_survey_poly_osam_run(void);
 
+/** Reset sweep number */
+extern void nav_survey_poly_osam_ResetSweepNumber(bool rst);
 #endif