Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
spiral.c
Go to the documentation of this file.
1 /************** Spiral Navigation **********************************************/
2 
10 
11 #include "subsystems/nav.h"
12 #include "estimator.h"
13 #include "autopilot.h"
14 #include "generated/flight_plan.h"
15 
16 #ifdef DIGITAL_CAM
17 #include "modules/digital_cam/dc.h"
18 #endif
19 
22 // static float SpiralTheta;
23 // static float Fly2X;
24 // static float Fly2Y;
25 
26 static float FlyFromX;
27 static float FlyFromY;
28 static float TransCurrentX;
29 static float TransCurrentY;
30 static float TransCurrentZ;
31 static float EdgeCurrentX;
32 static float EdgeCurrentY;
33 static float LastCircleX;
34 static float LastCircleY;
35 static float DistanceFromCenter;
36 static float Spiralradius;
37 static uint8_t Center;
38 static uint8_t Edge;
39 static float SRad;
40 static float IRad;
41 static float Alphalimit;
42 static float Segmente;
43 static float ZPoint;
44 static float nav_radius_min;
45 
46 #ifndef MIN_CIRCLE_RADIUS
47 #define MIN_CIRCLE_RADIUS 120
48 #endif
49 
50 
51 bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float IncRad, float Segments, float ZKoord)
52 {
53  Center = CenterWP; // center of the helix
54  Edge = EdgeWP; // edge point on the maximaum radius
55  SRad = StartRad; // start radius of the helix
56  Segmente = Segments;
57  ZPoint = ZKoord;
60  IRad = IncRad; // multiplier for increasing the spiral
61 
64 
66 
71 
72  // SpiralTheta = atan2(TransCurrentY,TransCurrentX);
73  // Fly2X = Spiralradius*cos(SpiralTheta+M_PI)+WaypointX(Center);
74  // Fly2Y = Spiralradius*sin(SpiralTheta+M_PI)+WaypointY(Center);
75 
76  // Alphalimit denotes angle, where the radius will be increased
77  Alphalimit = 2*M_PI / Segments;
78  //current position
81 
84  return FALSE;
85 }
86 
87 bool_t SpiralNav(void)
88 {
92 
93  float DistanceStartEstim;
94  float CircleAlpha;
95 
96  switch(CSpiralStatus)
97  {
98  case Outside:
99  //flys until center of the helix is reached an start helix
101  // center reached?
103  // nadir image
104 #ifdef DIGITAL_CAM
106 #endif
108  }
109  break;
110  case StartCircle:
111  // Starts helix
112  // storage of current coordinates
113  // calculation needed, State switch to Circle
115  if(DistanceFromCenter >= SRad){
119  // Start helix
120 #ifdef DIGITAL_CAM
121  dc_Circle(360/Segmente);
122 #endif
123  }
124  break;
125  case Circle: {
127  // Trigonometrische Berechnung des bereits geflogenen Winkels alpha
128  // equation:
129  // alpha = 2 * asin ( |Starting position angular - current positon| / (2* SRad)
130  // if alphamax already reached, increase radius.
131 
132  //DistanceStartEstim = |Starting position angular - current positon|
133  DistanceStartEstim = sqrt (((LastCircleX-estimator_x)*(LastCircleX-estimator_x))
135  CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * SRad)));
136  if (CircleAlpha >= Alphalimit) {
140  }
141  break;
142  }
143  case IncSpiral:
144  // increasing circle radius as long as it is smaller than max helix radius
145  if(SRad + IRad < Spiralradius)
146  {
147  SRad = SRad + IRad;
148 #ifdef DIGITAL_CAM
149  if (dc_cam_tracing) {
150  // calculating Cam angle for camera alignment
152  dc_cam_angle = atan(SRad/TransCurrentZ) * 180 / M_PI;
153  }
154 #endif
155  }
156  else {
157  SRad = Spiralradius;
158 #ifdef DIGITAL_CAM
159  // Stopps DC
160  dc_stop();
161 #endif
162  }
164  break;
165  default:
166  break;
167  }
168  return TRUE;
169 }
static enum SpiralStatus CSpiralStatus
Definition: spiral.c:21
static float EdgeCurrentY
Definition: spiral.c:32
static float FlyFromY
Definition: spiral.c:27
static float TransCurrentZ
Definition: spiral.c:30
static float DistanceFromCenter
Definition: spiral.c:35
float estimator_y
north position in meters
Definition: estimator.c:43
Definition: spiral.c:20
#define FALSE
Definition: imu_chimu.h:141
static float TransCurrentX
Definition: spiral.c:28
static float LastCircleX
Definition: spiral.c:33
Definition: dc.h:82
static float IRad
Definition: spiral.c:40
static float Alphalimit
Definition: spiral.c:41
static float FlyFromX
Definition: spiral.c:26
Standard Digital Camera Control Interface.
float estimator_x
east position in meters
Definition: estimator.c:42
static float nav_radius_min
Definition: spiral.c:44
bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float IncRad, float Segments, float ZKoord)
Definition: spiral.c:51
static uint8_t Edge
Definition: spiral.c:38
static float LastCircleY
Definition: spiral.c:34
#define TRUE
Definition: imu_chimu.h:144
static float SRad
Definition: spiral.c:39
void dc_send_command(uint8_t cmd)
static float ZPoint
Definition: spiral.c:43
static float TransCurrentY
Definition: spiral.c:29
unsigned char uint8_t
Definition: types.h:14
float estimator_z
altitude above MSL in meters
Definition: estimator.c:44
static float Segmente
Definition: spiral.c:42
#define MIN_CIRCLE_RADIUS
Definition: spiral.c:47
uint8_t dc_stop(void)
Sets the dc control in inactive mode, stopping all current actions.
Definition: dc.c:153
State estimation, fusioning sensors.
uint8_t dc_cam_tracing
Definition: dc.c:31
#define dc_Circle(interval)
Definition: dc.h:158
static uint8_t Center
Definition: spiral.c:37
SpiralStatus
creating a helix: start radius to end radius, increasing after reaching alphamax Alphamax is calculat...
Definition: spiral.c:20
bool_t SpiralNav(void)
Definition: spiral.c:87
static float Spiralradius
Definition: spiral.c:36
Definition: spiral.c:20
static float EdgeCurrentX
Definition: spiral.c:31
float dc_cam_angle
Definition: dc.c:32