Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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 bool_t 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  nav_spiral.radius_increment = radius_inc; // multiplier for increasing the spiral
60 
61  struct FloatVect2 edge;
62  VECT2_DIFF(edge, waypoints[edge_wp], nav_spiral.center);
63 
65 
66  // get a copy of the current position
67  struct EnuCoor_f pos_enu;
68  memcpy(&pos_enu, stateGetPositionEnu_f(), sizeof(struct EnuCoor_f));
69 
72 
74 
75  // nav_spiral.alpha_limit denotes angle, where the radius will be increased
77  //current position
80 
83  return FALSE;
84 }
85 
86 bool_t nav_spiral_run(void)
87 {
88  struct EnuCoor_f pos_enu;
89  memcpy(&pos_enu, stateGetPositionEnu_f(), sizeof(struct EnuCoor_f));
90 
93 
94  float DistanceStartEstim;
95  float CircleAlpha;
96 
97  switch(nav_spiral.status)
98  {
99  case SpiralOutside:
100  //flys until center of the helix is reached an start helix
102  // center reached?
104  // nadir image
105 #ifdef DIGITAL_CAM
107 #endif
109  }
110  break;
111  case SpiralStartCircle:
112  // Starts helix
113  // storage of current coordinates
114  // calculation needed, State switch to SpiralCircle
119  // Start helix
120 #ifdef DIGITAL_CAM
122 #endif
123  }
124  break;
125  case SpiralCircle: {
127  // Trigonometrische Berechnung des bereits geflogenen Winkels alpha
128  // equation:
129  // alpha = 2 * asin ( |Starting position angular - current positon| / (2* nav_spiral.radius_start)
130  // if alphamax already reached, increase radius.
131 
132  //DistanceStartEstim = |Starting position angular - current positon|
133  struct FloatVect2 pos_diff;
134  VECT2_DIFF(pos_diff, nav_spiral.last_circle, pos_enu);
135  FLOAT_VECT2_NORM(DistanceStartEstim, pos_diff);
136  CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * nav_spiral.radius_start)));
137  if (CircleAlpha >= nav_spiral.alpha_limit) {
140  }
141  break;
142  }
143  case SpiralInc:
144  // increasing circle radius as long as it is smaller than max helix radius
146  {
148 #ifdef DIGITAL_CAM
149  if (dc_cam_tracing) {
150  // calculating Cam angle for camera alignment
153  }
154 #endif
155  }
156  else {
158 #ifdef DIGITAL_CAM
159  // Stopps DC
160  dc_stop();
161 #endif
162  }
164  break;
165  default:
166  break;
167  }
168 
169  NavVerticalAutoThrottleMode(0.); /* No pitch */
170  NavVerticalAltitudeMode(nav_spiral.center.z, 0.); /* No preclimb */
171 
172  return TRUE;
173 }
float radius_increment
Definition: nav_spiral.h:48
struct FloatVect3 trans_current
Definition: nav_spiral.h:38
struct FloatVect2 fly_from
Definition: nav_spiral.h:39
float alpha_limit
Definition: nav_spiral.h:43
float segments
Definition: nav_spiral.h:44
struct FloatVect2 last_circle
Definition: nav_spiral.h:40
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:672
float radius_start
Definition: nav_spiral.h:47
float y
in meters
#define FALSE
Definition: imu_chimu.h:141
Definition: dc.h:82
#define FLOAT_VECT3_NORM(_v)
struct FloatVect3 center
Definition: nav_spiral.h:41
Standard Digital Camera Control Interface.
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:74
enum SpiralStatus status
Definition: nav_spiral.h:49
float x
in meters
float dist_from_center
Definition: nav_spiral.h:42
float radius
Definition: nav_spiral.h:45
#define TRUE
Definition: imu_chimu.h:144
void dc_send_command(uint8_t cmd)
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.
#define FLOAT_VECT2_NORM(_n, _v)
float radius_min
Definition: nav_spiral.h:46
uint8_t dc_stop(void)
Sets the dc control in inactive mode, stopping all current actions.
Definition: dc.c:150
vector in East North Up coordinates Units: meters
uint8_t dc_cam_tracing
Definition: dc.c:31
#define dc_Circle(interval)
Definition: dc.h:158
float alt
in meters above WGS84 reference ellipsoid
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:50
float dc_cam_angle
Definition: dc.c:32