[fixedwing] convert disc_survey to module

conf/firmwares/subsystems/fixedwing/navigation_extra.makefile

@@ -11,7 +11,6 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/common_flight_plan.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/traffic_info.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/nav_survey_rectangle.c $(SRC_SUBSYSTEMS)/navigation/nav_line.c
 
-$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/discsurvey.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/OSAMNav.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/snav.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/poly_survey_adv.c

conf/modules/nav_disc_survey.xml

@@ -0,0 +1,15 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="nav_disc_survey" dir="nav">
+  <doc>
+    <description>
+      Disc Survey.
+    </description>
+  </doc>
+  <header>
+    <file name="nav_disc_survey.h"/>
+  </header>
+  <makefile>
+    <file name="nav_disc_survey.c"/>
+  </makefile>
+</module>

sw/airborne/math/pprz_algebra_float.h

@@ -138,6 +138,7 @@ struct FloatRates {
     FLOAT_VECT2_SMUL(_v, _v, 1./n);             \
   }
 
+#define FLOAT_VECT2_DOT_PRODUCT(_v1, _v2) ((_v1).x*(_v2).x + (_v1).y*(_v2).y)
 
 /*
  * Dimension 3 Vectors

sw/airborne/modules/nav/nav_disc_survey.c

@@ -0,0 +1,122 @@
+/*
+ * Copyright (C) 2003-2005  Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file modules/nav/nav_disc_survey.c
+ *
+ */
+
+#include "modules/nav/nav_disc_survey.h"
+
+#include "generated/airframe.h"
+#include "state.h"
+#include "std.h"
+#include "subsystems/nav.h"
+#include "generated/flight_plan.h"
+#include "math/pprz_algebra_float.h"
+
+enum DiscSurveyStatus { UTURN, SEGMENT, DOWNWIND };
+
+struct DiscSurvey {
+  enum DiscSurveyStatus status;
+  int8_t sign;
+  struct FloatVect2 c;
+  struct FloatVect2 c1;
+  struct FloatVect2 c2;
+};
+
+static struct DiscSurvey disc_survey;
+
+
+bool_t nav_disc_survey_start( float grid ) {
+  nav_survey_shift = grid;
+  disc_survey.status = DOWNWIND;
+  disc_survey.sign = 1;
+  disc_survey.c1.x = stateGetPositionEnu_f()->x;
+  disc_survey.c1.y = stateGetPositionEnu_f()->y;
+  return FALSE;
+}
+
+bool_t nav_disc_survey_run( uint8_t center_wp, float radius) {
+  struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
+  float wind_dir = atan2(wind->x, wind->y) + M_PI;
+
+  /** Not null even if wind_east=wind_north=0 */
+  struct FloatVect2 upwind;
+  upwind.x = cos(wind_dir);
+  upwind.y = sin(wind_dir);
+
+  float grid = nav_survey_shift / 2;
+
+  switch (disc_survey.status) {
+  case UTURN:
+    nav_circle_XY(disc_survey.c.x, disc_survey.c.y, grid*disc_survey.sign);
+    if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) {
+      disc_survey.c1.x = stateGetPositionEnu_f()->x;
+      disc_survey.c1.y = stateGetPositionEnu_f()->y;
+
+      struct FloatVect2 dist;
+      VECT2_DIFF(dist, disc_survey.c1, waypoints[center_wp]);
+      float d = FLOAT_VECT2_DOT_PRODUCT(upwind, dist);
+      if (d > radius) {
+        disc_survey.status = DOWNWIND;
+      } else {
+        float w = sqrtf(radius*radius - d*d) - 1.5*grid;
+
+        struct FloatVect2 crosswind;
+        crosswind.x = -upwind.y;
+        crosswind.y = upwind.x;
+
+        disc_survey.c2.x = waypoints[center_wp].x + d*upwind.x - w*disc_survey.sign*crosswind.x;
+        disc_survey.c2.y = waypoints[center_wp].y + d*upwind.y - w*disc_survey.sign*crosswind.y;
+
+        disc_survey.status = SEGMENT;
+      }
+      nav_init_stage();
+    }
+    break;
+
+  case DOWNWIND:
+    disc_survey.c2.x = waypoints[center_wp].x - upwind.x * radius;
+    disc_survey.c2.y = waypoints[center_wp].y - upwind.y * radius;
+    disc_survey.status = SEGMENT;
+    /* No break; */
+
+  case SEGMENT:
+    nav_route_xy(disc_survey.c1.x, disc_survey.c1.y, disc_survey.c2.x, disc_survey.c2.y);
+    if (nav_approaching_xy(disc_survey.c2.x, disc_survey.c2.y, disc_survey.c1.x, disc_survey.c1.y, CARROT)) {
+      disc_survey.c.x = disc_survey.c2.x + grid*upwind.x;
+      disc_survey.c.y = disc_survey.c2.y + grid*upwind.y;
+
+      disc_survey.sign = -disc_survey.sign;
+      disc_survey.status = UTURN;
+      nav_init_stage();
+    }
+    break;
+  default:
+    break;
+  }
+
+  NavVerticalAutoThrottleMode(0.); /* No pitch */
+  NavVerticalAltitudeMode(WaypointAlt(center_wp), 0.); /* No preclimb */
+
+  return TRUE;
+}

sw/airborne/subsystems/navigation/discsurvey.h → sw/airborne/modules/nav/nav_disc_survey.h

@@ -20,16 +20,16 @@
  */
 
 /**
- * @file subsystems/navigation/discsurvey.h
+ * @file modules/nav/nav_disc_survey.h
  *
  */
 
-#ifndef DISCSURVEY_H
-#define DISCSURVEY_H
+#ifndef NAV_DISC_SURVEY_H
+#define NAV_DISC_SURVEY_H
 
 #include "std.h"
 
-extern bool_t disc_survey_init( float grid );
-extern bool_t disc_survey(uint8_t c, float radius);
+extern bool_t nav_disc_survey_start( float grid );
+extern bool_t nav_disc_survey_run(uint8_t c, float radius);
 
-#endif /* DISCSURVEY_H */
+#endif /* NAV_DISC_SURVEY_H */

sw/include/std.h

@@ -137,9 +137,6 @@ typedef uint8_t unit_t;
 
 #define Blend(a, b, rho) (((rho)*(a))+(1-(rho))*(b))
 
-#define ScalarProduct(x1,y1,x2,y2) ((x1)*(x2)+(y1)*(y2))
-
-
 #define RunOnceEvery(_prescaler, _code) {		\
     static uint16_t prescaler = 0;			\
     prescaler++;					\