Merge pull request #2489 from jburns11/master

Extending dynamic sweep width to poly_rotorcraft and poly_osam modules

conf/modules/gvf_module.xml

@@ -28,7 +28,7 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
       <define name="A" value="0" description="Amplitude for the sinusoidal y=Asin(Wx + OFF)" unit="m"/>
     </section>
   </doc>
-
+  <define name="NAV_SURVEY_POLY_GVF_DYNAMIC" value="FALSE|TRUE" description="Set to true to allow changing sweep distance mid polygon survey"/>
   <settings name="GVF">
     <dl_settings>
       <dl_settings NAME="GVF">

conf/modules/nav_survey_poly_osam.xml

@@ -14,6 +14,7 @@ You can use:
     <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"/>
+    <define name="NAV_SURVEY_POLY_OSAM_DYNAMIC" value="FALSE|TRUE" description="Set to true to allow changing sweep distance mid polygon survey"/>
   </doc>
 
   <settings>

conf/modules/nav_survey_poly_rotorcraft.xml

@@ -12,6 +12,7 @@ You can use:
     <define name="POLYSURVEY_DEFAULT_SIZE" value="5" description="default number of waypoints/corners used in polygon survey. Can be changed by telemetry"/>
     <define name="POLYSURVEY_DEFAULT_DISTANCE" value="120" description="default distance in m between lines. Can be changed by telemetry"/>
     <define name="POLYSURVEY_ENTRY_DISTANCE" value="4" description="distante between the polygon border and first line.If 0 the distance well be equal to half sweep"/>
+    <define name="NAV_SURVEY_POLY_DYNAMIC" value="FALSE|TRUE" description="Set to true to allow changing sweep distance mid polygon survey"/>
   </doc>
 
   <settings>
@@ -19,6 +20,7 @@ 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_rotorcraft"/>
         <dl_setting min="0" max="500" step="1" var="Poly_Distance" type="float" shortname="Sweep" module="modules/nav/nav_survey_poly_rotorcraft"/>
+        <dl_setting min="10" max="500" step="1" var="sweep_var" type="float" shortname="Distance" module="modules/nav/nav_survey_rectangle_rotorcraft"/>
       </dl_settings>
     </dl_settings>
   </settings>

sw/airborne/firmwares/fixedwing/nav.c

@@ -54,7 +54,7 @@ float carrot_x, carrot_y;
 float nav_circle_radians; /* Cumulated */
 float nav_circle_radians_no_rewind; /* Cumulated */
 float nav_circle_trigo_qdr; /* Angle from center to mobile */
-float nav_radius, nav_course, nav_climb, nav_shift;
+float nav_radius, nav_course, nav_climb, nav_shift, sweep_var;
 
 
 /** Status on the current leg (percentage, 0. < < 1.) in route mode */

sw/airborne/firmwares/fixedwing/nav.h

@@ -107,6 +107,7 @@ extern float nav_radius; /* m */
 extern float nav_course; /* degrees, clockwise, 0.0 = N */
 extern float nav_climb; /* m/s */
 extern float nav_shift; /* Lateral shift along a route. In meters */
+extern float sweep_var; //added to allow dynamic sweep width in surveys
 
 extern float nav_ground_speed_pgain, nav_ground_speed_setpoint;
 

sw/airborne/modules/guidance/gvf/nav/nav_survey_polygon_gvf.c

@@ -246,6 +246,10 @@ void gvf_nav_direction_circle(float rad)
 
 bool gvf_nav_survey_polygon_run(void)
 {
+  #ifdef NAV_SURVEY_POLY_GVF_DYNAMIC
+  sweep_width = (nav_survey_shift > 0 ? sweep_var : -sweep_var);
+  #endif
+
   NavVerticalAutoThrottleMode(0.0);
   NavVerticalAltitudeMode(gvf_survey.psa_altitude, 0.0);
 

sw/airborne/modules/nav/nav_survey_poly_osam.c

@@ -142,6 +142,7 @@ uint16_t PolySurveySweepNum;
 uint16_t PolySurveySweepBackNum;
 float EntryRadius;
 
+
 void nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float Orientation)
 {
   SmallestCorner.x = 0;
@@ -177,6 +178,7 @@ void nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float O
 
   SurveyEntryWP = EntryWP;
   SurveySize = Size;
+  sweep_var = sw;
 
   struct Point2D Corners[PolygonSize];
 
@@ -346,6 +348,10 @@ void nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float O
 
 bool nav_survey_poly_osam_run(void)
 {
+  #ifdef NAV_SURVEY_POLY_OSAM_DYNAMIC
+  dSweep = (nav_survey_shift > 0 ? sweep_var : -sweep_var);
+  #endif
+  
   struct Point2D C;
   struct Point2D ToP;
   struct Point2D FromP;

sw/airborne/modules/nav/nav_survey_poly_rotorcraft.c

@@ -59,7 +59,7 @@
 
 uint8_t Poly_Size = POLYSURVEY_DEFAULT_SIZE;
 float Poly_Distance = POLYSURVEY_DEFAULT_DISTANCE;
-
+float sweep_var;
 
 void nav_survey_poly_setup_towards(uint8_t FirstWP, uint8_t Size, float Sweep, int SecondWP)
 {
@@ -149,6 +149,7 @@ void nav_survey_poly_setup(uint8_t EntryWP, uint8_t Size, float sw, float Orient
 
   SurveyEntryWP = EntryWP;
   SurveySize = Size;
+  sweep_var = sw;
 
   struct EnuCoor_f Corners[MaxPolygonSize];
 
@@ -307,6 +308,10 @@ void nav_survey_poly_setup(uint8_t EntryWP, uint8_t Size, float sw, float Orient
 bool nav_survey_poly_run(void)
 {
 
+  #ifdef NAV_SURVEY_POLY_DYNAMIC
+  dSweep = (nav_survey_shift > 0 ? sweep_var : -sweep_var);
+  #endif 
+
   struct EnuCoor_f C;
   struct EnuCoor_f ToP;
   struct EnuCoor_f FromP;

sw/airborne/modules/nav/nav_survey_poly_rotorcraft.h

@@ -31,6 +31,7 @@
 
 extern uint8_t Poly_Size;
 extern float Poly_Distance;
+extern float sweep_var;
 extern uint16_t PolySurveySweepNum;
 extern uint16_t PolySurveySweepBackNum;
 extern bool Half_Sweep_Enabled;

sw/airborne/subsystems/navigation/nav_survey_rectangle.c

@@ -34,7 +34,6 @@ static struct point survey_from;
 static struct point survey_to;
 static bool survey_uturn __attribute__((unused)) = false;
 static survey_orientation_t survey_orientation = NS;
-float sweep_var;
 
 #define SurveyGoingNorth() ((survey_orientation == NS) && (survey_to.y > survey_from.y))
 #define SurveyGoingSouth() ((survey_orientation == NS) && (survey_to.y < survey_from.y))
@@ -91,7 +90,7 @@ void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orie
 void nav_survey_rectangle(uint8_t wp1, uint8_t wp2)
 {
   #ifdef NAV_SURVEY_RECTANGLE_DYNAMIC
-  nav_survey_shift = (nav_survey_shift > 0 ? sweep : -sweep);
+  nav_survey_shift = (nav_survey_shift > 0 ? sweep_var : -sweep_var);
   #endif
 
   static float survey_radius;