Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nav_spiral.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011-2013 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 
33 #include "modules/nav/nav_spiral.h"
34 
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 
44 #ifndef NAV_SPIRAL_MIN_CIRCLE_RADIUS
45 #define NAV_SPIRAL_MIN_CIRCLE_RADIUS 60
46 #endif
47 
49 
50 void nav_spiral_setup(uint8_t center_wp, uint8_t edge_wp, float radius_start, float radius_inc, float segments)
51 {
52  VECT2_COPY(nav_spiral.center, waypoints[center_wp]); // center of the helix
53  nav_spiral.center.z = waypoints[center_wp].a;
54  nav_spiral.radius_start = radius_start; // start radius of the helix
59  }
60  nav_spiral.radius_increment = radius_inc; // multiplier for increasing the spiral
61 
62  struct FloatVect2 edge;
63  VECT2_DIFF(edge, waypoints[edge_wp], nav_spiral.center);
64 
66 
67  // get a copy of the current position
68  struct EnuCoor_f pos_enu = *stateGetPositionEnu_f();
69 
72 
74 
75  // nav_spiral.alpha_limit denotes angle, where the radius will be increased
77  //current position
80 
83  }
84 }
85 
86 bool nav_spiral_run(void)
87 {
88  struct EnuCoor_f pos_enu = *stateGetPositionEnu_f();
89 
92 
93  float DistanceStartEstim;
94  float CircleAlpha;
95 
96  switch (nav_spiral.status) {
97  case SpiralOutside:
98  //flys until center of the helix is reached an start helix
100  // center reached?
102  // nadir image
103 #ifdef DIGITAL_CAM
105 #endif
107  }
108  break;
109  case SpiralStartCircle:
110  // Starts helix
111  // storage of current coordinates
112  // calculation needed, State switch to SpiralCircle
117  // Start helix
118 #ifdef DIGITAL_CAM
120 #endif
121  }
122  break;
123  case SpiralCircle: {
125  // Trigonometrische Berechnung des bereits geflogenen Winkels alpha
126  // equation:
127  // alpha = 2 * asin ( |Starting position angular - current positon| / (2* nav_spiral.radius_start)
128  // if alphamax already reached, increase radius.
129 
130  //DistanceStartEstim = |Starting position angular - current positon|
131  struct FloatVect2 pos_diff;
132  VECT2_DIFF(pos_diff, nav_spiral.last_circle, pos_enu);
133  DistanceStartEstim = float_vect2_norm(&pos_diff);
134  CircleAlpha = (2.0 * asin(DistanceStartEstim / (2 * nav_spiral.radius_start)));
135  if (CircleAlpha >= nav_spiral.alpha_limit) {
138  }
139  break;
140  }
141  case SpiralInc:
142  // increasing circle radius as long as it is smaller than max helix radius
145 #ifdef DIGITAL_CAM
146  if (dc_cam_tracing) {
147  // calculating Cam angle for camera alignment
150  }
151 #endif
152  } else {
154 #ifdef DIGITAL_CAM
155  // Stopps DC
156  dc_stop();
157 #endif
158  }
160  break;
161  default:
162  break;
163  }
164 
165  NavVerticalAutoThrottleMode(0.); /* No pitch */
166  NavVerticalAltitudeMode(nav_spiral.center.z, 0.); /* No preclimb */
167 
168  return true;
169 }
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
vector in East North Up coordinates Units: meters
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:67
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:91
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
#define FLOAT_VECT3_NORM(_v)
Definition: dc.h:100
Standard Digital Camera Control Interface.
static float float_vect2_norm(struct FloatVect2 *v)
float x
in meters
Core autopilot interface common to all firmwares.
void dc_send_command(uint8_t cmd)
Send Command To Camera.
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
uint8_t dc_stop(void)
Stop dc control.
Definition: dc.c:223
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
float a
Definition: common_nav.h:42
uint8_t dc_cam_tracing
Definition: dc.c:64
#define dc_Circle(interval)
Definition: dc.h:188
float y
in meters
float dc_cam_angle
camera angle
Definition: dc.c:65