diff --git a/conf/airframes/examples/Twinstar_energyadaptive.xml b/conf/airframes/examples/Twinstar_energyadaptive.xml
index 47145fa40e..694fbd9aa1 100644
--- a/conf/airframes/examples/Twinstar_energyadaptive.xml
+++ b/conf/airframes/examples/Twinstar_energyadaptive.xml
@@ -164,7 +164,6 @@ twog_1.0 + aspirin + ETS baro + ETS speed
-
diff --git a/conf/airframes/flixr_discovery.xml b/conf/airframes/flixr_discovery.xml
index fcc9e9ae0e..c0571178e8 100644
--- a/conf/airframes/flixr_discovery.xml
+++ b/conf/airframes/flixr_discovery.xml
@@ -227,9 +227,6 @@
-
-
-
diff --git a/conf/firmwares/subsystems/fixedwing/navigation_extra.makefile b/conf/firmwares/subsystems/fixedwing/navigation_extra.makefile
index 91828035b2..e4e33e1123 100644
--- a/conf/firmwares/subsystems/fixedwing/navigation_extra.makefile
+++ b/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
diff --git a/conf/modules/nav_spiral.xml b/conf/modules/nav_spiral.xml
new file mode 100644
index 0000000000..babde6d9f3
--- /dev/null
+++ b/conf/modules/nav_spiral.xml
@@ -0,0 +1,20 @@
+
+
+
+
+
+ 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!
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/modules/nav/nav_spiral.c b/sw/airborne/modules/nav/nav_spiral.c
new file mode 100644
index 0000000000..5c3703040b
--- /dev/null
+++ b/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;
+}
diff --git a/sw/airborne/modules/nav/nav_spiral.h b/sw/airborne/modules/nav/nav_spiral.h
new file mode 100644
index 0000000000..a9a97ba6ed
--- /dev/null
+++ b/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
diff --git a/sw/airborne/subsystems/navigation/spiral.c b/sw/airborne/subsystems/navigation/spiral.c
deleted file mode 100644
index 11e004d3f7..0000000000
--- a/sw/airborne/subsystems/navigation/spiral.c
+++ /dev/null
@@ -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;
-}
diff --git a/sw/airborne/subsystems/navigation/spiral.h b/sw/airborne/subsystems/navigation/spiral.h
deleted file mode 100644
index d49257a171..0000000000
--- a/sw/airborne/subsystems/navigation/spiral.h
+++ /dev/null
@@ -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
-