[fixewing][modules] convert nav_spiral to module

conf/airframes/examples/Twinstar_energyadaptive.xml

@@ -164,7 +164,6 @@ twog_1.0 + aspirin + ETS baro + ETS speed
     <define name="NO_XBEE_API_INIT" value="TRUE"/>
     <define name="TRIGGER_DELAY" value="1."/>
     <define name="DEFAULT_CIRCLE_RADIUS" value="70."/>
-    <define name="MIN_CIRCLE_RADIUS" value="35.0"/>
   </section>
 
   <section name="GLS_APPROACH" prefix="APP_">

conf/airframes/flixr_discovery.xml

@@ -227,9 +227,6 @@
 
     <define name="DEFAULT_CIRCLE_RADIUS" value="50.0"/>
 
-    <!-- only for spiral -->
-    <define name="MIN_CIRCLE_RADIUS" value="35.0"/>
-
     <!-- The Glide definitions are used for calculating the touch down point during auto landing -->
     <!--
     -->

conf/firmwares/subsystems/fixedwing/navigation_extra.makefile

@@ -14,7 +14,6 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/nav_survey_rectangle.c $(SRC_SUBS
 $(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/spiral.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/poly_survey_adv.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/gls.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/border_line.c

conf/modules/nav_spiral.xml

@@ -0,0 +1,20 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="nav_spiral" dir="nav">
+  <doc>
+    <description>
+      Fixedwing navigation in a spiral/helix.
+      creating a helix:
+      - start radius to end radius, increasing after reaching alphamax
+      - Alphamax is calculated from given segments
+      - IMPORTANT: numer of segments has to be larger than 2!
+    </description>
+    <define name="NAV_SPIRAL_MIN_CIRCLE_RADIUS" value="120" description="minium circle radius in meters"/>
+  </doc>
+  <header>
+    <file name="nav_spiral.h"/>
+  </header>
+  <makefile>
+    <file name="nav_spiral.c"/>
+  </makefile>
+</module>

sw/airborne/modules/nav/nav_spiral.c

@@ -0,0 +1,172 @@
+/*
+ * Copyright (C) 2011-2013  The Paparazzi Team
+ *
+ * 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_spiral.c
+ *
+ * Fixedwing navigation in a spiral/helix.
+ *
+ * creating a helix:
+ * - start radius to end radius, increasing after reaching alphamax
+ * - Alphamax is calculated from given segments
+ * - IMPORTANT: numer of segments has to be larger than 2!
+ */
+
+#include "modules/nav/nav_spiral.h"
+
+#include "subsystems/nav.h"
+#include "state.h"
+#include "autopilot.h"
+#include "generated/flight_plan.h"
+
+#ifdef DIGITAL_CAM
+#include "modules/digital_cam/dc.h"
+#endif
+
+#ifndef NAV_SPIRAL_MIN_CIRCLE_RADIUS
+#define NAV_SPIRAL_MIN_CIRCLE_RADIUS 60
+#endif
+
+struct NavSpiral nav_spiral;
+
+bool_t nav_spiral_start(uint8_t center_wp, uint8_t edge_wp, float radius_start, float radius_inc, float segments)
+{
+  VECT2_COPY(nav_spiral.center, waypoints[center_wp]);    // center of the helix
+  nav_spiral.center.z = waypoints[center_wp].a;
+  nav_spiral.radius_start = radius_start;   // start radius of the helix
+  nav_spiral.segments = segments;
+  nav_spiral.radius_min = NAV_SPIRAL_MIN_CIRCLE_RADIUS;
+  if (nav_spiral.radius_start < nav_spiral.radius_min)
+    nav_spiral.radius_start = nav_spiral.radius_min;
+  nav_spiral.radius_increment = radius_inc;     // multiplier for increasing the spiral
+
+  struct FloatVect2 edge;
+  VECT2_DIFF(edge, waypoints[edge_wp], nav_spiral.center);
+
+  FLOAT_VECT2_NORM(nav_spiral.radius, edge);
+
+  // get a copy of the current position
+  struct EnuCoor_f pos_enu;
+  memcpy(&pos_enu, stateGetPositionEnu_f(), sizeof(struct EnuCoor_f));
+
+  VECT3_DIFF(nav_spiral.trans_current, pos_enu, nav_spiral.center);
+
+  nav_spiral.dist_from_center = FLOAT_VECT3_NORM(nav_spiral.trans_current);
+
+  // nav_spiral.alpha_limit denotes angle, where the radius will be increased
+  nav_spiral.alpha_limit = 2*M_PI / nav_spiral.segments;
+  //current position
+  nav_spiral.fly_from.x = stateGetPositionEnu_f()->x;
+  nav_spiral.fly_from.y = stateGetPositionEnu_f()->y;
+
+  if(nav_spiral.dist_from_center > nav_spiral.radius)
+    nav_spiral.status = Outside;
+  return FALSE;
+}
+
+bool_t nav_spiral_run(void)
+{
+  struct EnuCoor_f pos_enu;
+  memcpy(&pos_enu, stateGetPositionEnu_f(), sizeof(struct EnuCoor_f));
+
+  VECT2_DIFF(nav_spiral.trans_current, pos_enu, nav_spiral.center);
+  nav_spiral.dist_from_center = FLOAT_VECT3_NORM(nav_spiral.trans_current);
+
+  float DistanceStartEstim;
+  float CircleAlpha;
+
+  switch(nav_spiral.status)
+  {
+    case Outside:
+      //flys until center of the helix is reached an start helix
+      nav_route_xy(nav_spiral.fly_from.x, nav_spiral.fly_from.y, nav_spiral.center.x, nav_spiral.center.y);
+      // center reached?
+      if (nav_approaching_xy(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.fly_from.x, nav_spiral.fly_from.y, 0)) {
+        // nadir image
+#ifdef DIGITAL_CAM
+        dc_send_command(DC_SHOOT);
+#endif
+        nav_spiral.status = StartCircle;
+      }
+      break;
+    case StartCircle:
+      // Starts helix
+      // storage of current coordinates
+      // calculation needed, State switch to Circle
+      nav_circle_XY(nav_spiral.center.y, nav_spiral.center.y, nav_spiral.radius_start);
+      if(nav_spiral.dist_from_center >= nav_spiral.radius_start){
+        VECT2_COPY(nav_spiral.last_circle, pos_enu);
+        nav_spiral.status = Circle;
+        // Start helix
+#ifdef DIGITAL_CAM
+        dc_Circle(360/nav_spiral.segments);
+#endif
+      }
+      break;
+    case Circle: {
+      nav_circle_XY(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.radius_start);
+      // Trigonometrische Berechnung des bereits geflogenen Winkels alpha
+      // equation:
+      // alpha = 2 * asin ( |Starting position angular - current positon| / (2* nav_spiral.radius_start)
+      // if alphamax already reached, increase radius.
+
+      //DistanceStartEstim = |Starting position angular - current positon|
+      struct FloatVect2 pos_diff;
+      VECT2_DIFF(pos_diff, nav_spiral.last_circle, pos_enu);
+      FLOAT_VECT2_NORM(DistanceStartEstim, pos_diff);
+      CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * nav_spiral.radius_start)));
+      if (CircleAlpha >= nav_spiral.alpha_limit) {
+        VECT2_COPY(nav_spiral.last_circle, pos_enu);
+        nav_spiral.status = IncSpiral;
+      }
+      break;
+    }
+    case IncSpiral:
+      // increasing circle radius as long as it is smaller than max helix radius
+      if(nav_spiral.radius_start + nav_spiral.radius_increment < nav_spiral.radius)
+      {
+        nav_spiral.radius_start = nav_spiral.radius_start + nav_spiral.radius_increment;
+#ifdef DIGITAL_CAM
+        if (dc_cam_tracing) {
+          // calculating Cam angle for camera alignment
+          nav_spiral.trans_current.z = pos_enu.z - nav_spiral.center.z;
+          dc_cam_angle = atan(nav_spiral.radius_start/nav_spiral.trans_current.z) * 180  / M_PI;
+        }
+#endif
+      }
+      else {
+        nav_spiral.radius_start = nav_spiral.radius;
+#ifdef DIGITAL_CAM
+        // Stopps DC
+        dc_stop();
+#endif
+      }
+      nav_spiral.status = Circle;
+      break;
+    default:
+      break;
+  }
+
+  NavVerticalAutoThrottleMode(0.); /* No pitch */
+  NavVerticalAltitudeMode(nav_spiral.center.z, 0.); /* No preclimb */
+
+  return TRUE;
+}

sw/airborne/modules/nav/nav_spiral.h

@@ -0,0 +1,58 @@
+/*
+ * Copyright (C) 2011-2013  The Paparazzi Team
+ *
+ * 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_spiral.h
+ *
+ * Fixedwing navigation in a spiral/helix.
+ *
+ */
+
+#ifndef NAV_SPIRAL_H
+#define NAV_SPIRAL_H
+
+#include "std.h"
+#include "math/pprz_algebra_float.h"
+
+enum SpiralStatus { Outside, StartCircle, Circle, IncSpiral };
+
+struct NavSpiral {
+  struct FloatVect3 trans_current;
+  struct FloatVect2 fly_from;
+  struct FloatVect2 last_circle;
+  struct FloatVect3 center;
+  float dist_from_center;
+  float alpha_limit;
+  float segments;
+  float radius;
+  float radius_min;
+  float radius_start;
+  float radius_increment;
+  enum SpiralStatus status;
+};
+
+extern struct NavSpiral nav_spiral;
+
+extern bool_t nav_spiral_run(void);
+extern bool_t nav_spiral_start(uint8_t center_wp, uint8_t edge_wp, float radius_start,
+                       float radius_inc, float segments);
+
+#endif // NAV_SPIRAL_H

sw/airborne/subsystems/navigation/spiral.c

@@ -1,193 +0,0 @@
-/*
- * Copyright (C) 2011  The Paparazzi Team
- *
- * 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 subsystems/navigation/spiral.c
- *
- * Fixedwing navigation in a spiral/helix from Uni Stuttgart.
- *
- * creating a helix:
- * - start radius to end radius, increasing after reaching alphamax
- * - Alphamax is calculated from given segments
- * - IMPORTANT: numer of segments has to be larger than 2!
- */
-
-#include "subsystems/navigation/spiral.h"
-
-#include "subsystems/nav.h"
-#include "state.h"
-#include "autopilot.h"
-#include "generated/flight_plan.h"
-
-#ifdef DIGITAL_CAM
-#include "modules/digital_cam/dc.h"
-#endif
-
-enum SpiralStatus { Outside, StartCircle, Circle, IncSpiral };
-static enum SpiralStatus CSpiralStatus;
-// static float SpiralTheta;
-// static float Fly2X;
-// static float Fly2Y;
-
-static float FlyFromX;
-static float FlyFromY;
-static float TransCurrentX;
-static float TransCurrentY;
-static float TransCurrentZ;
-static float EdgeCurrentX;
-static float EdgeCurrentY;
-static float LastCircleX;
-static float LastCircleY;
-static float DistanceFromCenter;
-static float Spiralradius;
-static uint8_t Center;
-static uint8_t Edge;
-static float SRad;
-static float IRad;
-static float Alphalimit;
-static float Segmente;
-static float ZPoint;
-static float nav_radius_min;
-
-#ifndef MIN_CIRCLE_RADIUS
-#define MIN_CIRCLE_RADIUS 120
-#endif
-
-
-bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float IncRad, float Segments, float ZKoord)
-{
-  Center = CenterWP;    // center of the helix
-  Edge = EdgeWP;        // edge point on the maximaum radius
-  SRad = StartRad;	// start radius of the helix
-  Segmente = Segments;
-  ZPoint = ZKoord;
-  nav_radius_min = MIN_CIRCLE_RADIUS;
-  if (SRad < nav_radius_min) SRad = nav_radius_min;
-  IRad = IncRad;		// multiplier for increasing the spiral
-
-  EdgeCurrentX = WaypointX(Edge) - WaypointX(Center);
-  EdgeCurrentY = WaypointY(Edge) - WaypointY(Center);
-
-  Spiralradius = sqrt(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY);
-
-  TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center);
-  TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center);
-  TransCurrentZ = stateGetPositionEnu_f()->z - ZPoint;
-  DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
-
-  //    SpiralTheta = atan2(TransCurrentY,TransCurrentX);
-  //    Fly2X = Spiralradius*cos(SpiralTheta+M_PI)+WaypointX(Center);
-  //    Fly2Y = Spiralradius*sin(SpiralTheta+M_PI)+WaypointY(Center);
-
-  // Alphalimit denotes angle, where the radius will be increased
-  Alphalimit = 2*M_PI / Segments;
-  //current position
-  FlyFromX = stateGetPositionEnu_f()->x;
-  FlyFromY = stateGetPositionEnu_f()->y;
-
-  if(DistanceFromCenter > Spiralradius)
-    CSpiralStatus = Outside;
-  return FALSE;
-}
-
-bool_t SpiralNav(void)
-{
-  TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center);
-  TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center);
-  DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
-
-  float DistanceStartEstim;
-  float CircleAlpha;
-
-  switch(CSpiralStatus)
-  {
-  case Outside:
-    //flys until center of the helix is reached an start helix
-    nav_route_xy(FlyFromX,FlyFromY,WaypointX(Center), WaypointY(Center));
-    // center reached?
-    if (nav_approaching_xy(WaypointX(Center), WaypointY(Center), FlyFromX, FlyFromY, 0)) {
-      // nadir image
-#ifdef DIGITAL_CAM
-      dc_send_command(DC_SHOOT);
-#endif
-      CSpiralStatus = StartCircle;
-    }
-    break;
-  case StartCircle:
-    // Starts helix
-    // storage of current coordinates
-    // calculation needed, State switch to Circle
-    nav_circle_XY(WaypointX(Center), WaypointY(Center), SRad);
-    if(DistanceFromCenter >= SRad){
-      LastCircleX = stateGetPositionEnu_f()->x;
-      LastCircleY = stateGetPositionEnu_f()->y;
-      CSpiralStatus = Circle;
-      // Start helix
-#ifdef DIGITAL_CAM
-      dc_Circle(360/Segmente);
-#endif
-    }
-    break;
-  case Circle: {
-    nav_circle_XY(WaypointX(Center), WaypointY(Center), SRad);
-    // Trigonometrische Berechnung des bereits geflogenen Winkels alpha
-    // equation:
-    // alpha = 2 * asin ( |Starting position angular - current positon| / (2* SRad)
-    // if alphamax already reached, increase radius.
-
-    //DistanceStartEstim = |Starting position angular - current positon|
-    DistanceStartEstim = sqrt (((LastCircleX-stateGetPositionEnu_f()->x)*(LastCircleX-stateGetPositionEnu_f()->x))
-                               + ((LastCircleY-stateGetPositionEnu_f()->y)*(LastCircleY-stateGetPositionEnu_f()->y)));
-    CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * SRad)));
-    if (CircleAlpha >= Alphalimit) {
-      LastCircleX = stateGetPositionEnu_f()->x;
-      LastCircleY = stateGetPositionEnu_f()->y;
-      CSpiralStatus = IncSpiral;
-    }
-    break;
-  }
-  case IncSpiral:
-    // increasing circle radius as long as it is smaller than max helix radius
-    if(SRad + IRad < Spiralradius)
-    {
-      SRad = SRad + IRad;
-#ifdef DIGITAL_CAM
-      if (dc_cam_tracing) {
-        // calculating Cam angle for camera alignment
-        TransCurrentZ = stateGetPositionEnu_f()->z - ZPoint;
-        dc_cam_angle = atan(SRad/TransCurrentZ) * 180  / M_PI;
-      }
-#endif
-    }
-    else {
-      SRad = Spiralradius;
-#ifdef DIGITAL_CAM
-      // Stopps DC
-      dc_stop();
-#endif
-    }
-    CSpiralStatus = Circle;
-    break;
-  default:
-    break;
-  }
-  return TRUE;
-}

sw/airborne/subsystems/navigation/spiral.h

@@ -1,39 +0,0 @@
-/*
- * Copyright (C) 2011  The Paparazzi Team
- *
- * 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 subsystems/navigation/spiral.h
- *
- * Fixedwing navigation in a spiral/helix from Uni Stuttgart.
- *
- */
-
-#ifndef SPIRAL_H
-#define SPIRAL_H
-
-#include "std.h"
-
-extern bool_t SpiralNav(void);
-extern bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float IncRad,
-        float Segments, float ZKoord );
-
-#endif // SPIRAL_H
-