Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
48 struct NavSpiral nav_spiral;
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;
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 }
void dc_send_command(uint8_t cmd)
Send Command To Camera.
Core autopilot interface common to all firmwares.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
float a
Definition: common_nav.h:42
uint8_t dc_stop(void)
Stop dc control.
Definition: dc.c:257
uint8_t dc_cam_tracing
Definition: dc.c:69
float dc_cam_angle
camera angle
Definition: dc.c:70
Standard Digital Camera Control Interface.
#define dc_Circle(interval)
Definition: dc.h:191
@ DC_SHOOT
Definition: dc.h:100
#define FLOAT_VECT3_NORM(_v)
static float float_vect2_norm(struct FloatVect2 *v)
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
bool nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time)
Decide if the UAV is approaching the current waypoint.
Definition: nav.c:325
void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y)
Computes the carrot position along the desired segment.
Definition: nav.c:382
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Definition: nav.c:108
Fixedwing Navigation library.
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition: nav.h:191
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition: nav.h:177
void nav_spiral_setup(uint8_t center_wp, uint8_t edge_wp, float radius_start, float radius_inc, float segments)
Definition: nav_spiral.c:50
struct NavSpiral nav_spiral
Definition: nav_spiral.c:48
#define NAV_SPIRAL_MIN_CIRCLE_RADIUS
Definition: nav_spiral.c:45
bool nav_spiral_run(void)
Definition: nav_spiral.c:86
Fixedwing navigation in a spiral/helix.
float radius
Definition: nav_spiral.h:45
float radius_min
Definition: nav_spiral.h:46
enum SpiralStatus status
Definition: nav_spiral.h:49
float alpha_limit
Definition: nav_spiral.h:43
float radius_increment
Definition: nav_spiral.h:48
struct FloatVect3 trans_current
Definition: nav_spiral.h:38
@ SpiralOutside
Definition: nav_spiral.h:35
@ SpiralInc
Definition: nav_spiral.h:35
@ SpiralStartCircle
Definition: nav_spiral.h:35
@ SpiralCircle
Definition: nav_spiral.h:35
float radius_start
Definition: nav_spiral.h:47
struct FloatVect3 center
Definition: nav_spiral.h:41
float segments
Definition: nav_spiral.h:44
struct FloatVect2 fly_from
Definition: nav_spiral.h:39
float dist_from_center
Definition: nav_spiral.h:42
struct FloatVect2 last_circle
Definition: nav_spiral.h:40
struct FloatVect2 pos_diff
float y
in meters
float x
in meters
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
vector in East North Up coordinates Units: meters
API to get/set the generic vehicle states.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98