Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
spiral.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011 The Paparazzi Team
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
34 
35 #include "subsystems/nav.h"
36 #include "state.h"
37 #include "autopilot.h"
38 #include "generated/flight_plan.h"
39 
40 #ifdef DIGITAL_CAM
41 #include "modules/digital_cam/dc.h"
42 #endif
43 
46 // static float SpiralTheta;
47 // static float Fly2X;
48 // static float Fly2Y;
49 
50 static float FlyFromX;
51 static float FlyFromY;
52 static float TransCurrentX;
53 static float TransCurrentY;
54 static float TransCurrentZ;
55 static float EdgeCurrentX;
56 static float EdgeCurrentY;
57 static float LastCircleX;
58 static float LastCircleY;
59 static float DistanceFromCenter;
60 static float Spiralradius;
61 static uint8_t Center;
62 static uint8_t Edge;
63 static float SRad;
64 static float IRad;
65 static float Alphalimit;
66 static float Segmente;
67 static float ZPoint;
68 static float nav_radius_min;
69 
70 #ifndef MIN_CIRCLE_RADIUS
71 #define MIN_CIRCLE_RADIUS 120
72 #endif
73 
74 
75 bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float IncRad, float Segments, float ZKoord)
76 {
77  Center = CenterWP; // center of the helix
78  Edge = EdgeWP; // edge point on the maximaum radius
79  SRad = StartRad; // start radius of the helix
80  Segmente = Segments;
81  ZPoint = ZKoord;
84  IRad = IncRad; // multiplier for increasing the spiral
85 
88 
90 
95 
96  // SpiralTheta = atan2(TransCurrentY,TransCurrentX);
97  // Fly2X = Spiralradius*cos(SpiralTheta+M_PI)+WaypointX(Center);
98  // Fly2Y = Spiralradius*sin(SpiralTheta+M_PI)+WaypointY(Center);
99 
100  // Alphalimit denotes angle, where the radius will be increased
101  Alphalimit = 2*M_PI / Segments;
102  //current position
105 
108  return FALSE;
109 }
110 
111 bool_t SpiralNav(void)
112 {
116 
117  float DistanceStartEstim;
118  float CircleAlpha;
119 
120  switch(CSpiralStatus)
121  {
122  case Outside:
123  //flys until center of the helix is reached an start helix
125  // center reached?
127  // nadir image
128 #ifdef DIGITAL_CAM
130 #endif
132  }
133  break;
134  case StartCircle:
135  // Starts helix
136  // storage of current coordinates
137  // calculation needed, State switch to Circle
139  if(DistanceFromCenter >= SRad){
143  // Start helix
144 #ifdef DIGITAL_CAM
145  dc_Circle(360/Segmente);
146 #endif
147  }
148  break;
149  case Circle: {
151  // Trigonometrische Berechnung des bereits geflogenen Winkels alpha
152  // equation:
153  // alpha = 2 * asin ( |Starting position angular - current positon| / (2* SRad)
154  // if alphamax already reached, increase radius.
155 
156  //DistanceStartEstim = |Starting position angular - current positon|
157  DistanceStartEstim = sqrt (((LastCircleX-stateGetPositionEnu_f()->x)*(LastCircleX-stateGetPositionEnu_f()->x))
159  CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * SRad)));
160  if (CircleAlpha >= Alphalimit) {
164  }
165  break;
166  }
167  case IncSpiral:
168  // increasing circle radius as long as it is smaller than max helix radius
169  if(SRad + IRad < Spiralradius)
170  {
171  SRad = SRad + IRad;
172 #ifdef DIGITAL_CAM
173  if (dc_cam_tracing) {
174  // calculating Cam angle for camera alignment
176  dc_cam_angle = atan(SRad/TransCurrentZ) * 180 / M_PI;
177  }
178 #endif
179  }
180  else {
181  SRad = Spiralradius;
182 #ifdef DIGITAL_CAM
183  // Stopps DC
184  dc_stop();
185 #endif
186  }
188  break;
189  default:
190  break;
191  }
192  return TRUE;
193 }
static enum SpiralStatus CSpiralStatus
Definition: spiral.c:45
static float EdgeCurrentY
Definition: spiral.c:56
static float FlyFromY
Definition: spiral.c:51
static float TransCurrentZ
Definition: spiral.c:54
static float DistanceFromCenter
Definition: spiral.c:59
Fixedwing navigation in a spiral/helix from Uni Stuttgart.
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:672
float y
in meters
Definition: spiral.c:44
#define FALSE
Definition: imu_chimu.h:141
static float TransCurrentX
Definition: spiral.c:52
static float LastCircleX
Definition: spiral.c:57
Definition: dc.h:82
static float IRad
Definition: spiral.c:64
static float Alphalimit
Definition: spiral.c:65
static float FlyFromX
Definition: spiral.c:50
Standard Digital Camera Control Interface.
float x
in meters
static float nav_radius_min
Definition: spiral.c:68
bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float IncRad, float Segments, float ZKoord)
Definition: spiral.c:75
static uint8_t Edge
Definition: spiral.c:62
static float LastCircleY
Definition: spiral.c:58
#define TRUE
Definition: imu_chimu.h:144
static float SRad
Definition: spiral.c:63
void dc_send_command(uint8_t cmd)
static float ZPoint
Definition: spiral.c:67
static float TransCurrentY
Definition: spiral.c:53
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:651
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
static float Segmente
Definition: spiral.c:66
#define MIN_CIRCLE_RADIUS
Definition: spiral.c:71
uint8_t dc_stop(void)
Sets the dc control in inactive mode, stopping all current actions.
Definition: dc.c:153
float y
Definition: common_nav.h:41
uint8_t dc_cam_tracing
Definition: dc.c:31
#define dc_Circle(interval)
Definition: dc.h:158
static uint8_t Center
Definition: spiral.c:61
SpiralStatus
Definition: spiral.c:44
bool_t SpiralNav(void)
Definition: spiral.c:111
static float Spiralradius
Definition: spiral.c:60
Definition: spiral.c:44
float alt
in meters above WGS84 reference ellipsoid
static float EdgeCurrentX
Definition: spiral.c:55
float x
Definition: common_nav.h:40
float dc_cam_angle
Definition: dc.c:32