Paparazzi UAS  v5.18.0_stable
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 
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 }
point::a
float a
Definition: common_nav.h:42
NavSpiral::radius
float radius
Definition: nav_spiral.h:45
SpiralStartCircle
@ SpiralStartCircle
Definition: nav_spiral.h:35
nav_spiral
struct NavSpiral nav_spiral
Definition: nav_spiral.c:48
dc_cam_tracing
uint8_t dc_cam_tracing
Definition: dc.c:69
NavSpiral::fly_from
struct FloatVect2 fly_from
Definition: nav_spiral.h:39
dc_Circle
#define dc_Circle(interval)
Definition: dc.h:191
NavSpiral::radius_increment
float radius_increment
Definition: nav_spiral.h:48
SpiralOutside
@ SpiralOutside
Definition: nav_spiral.h:35
NavSpiral::alpha_limit
float alpha_limit
Definition: nav_spiral.h:43
SpiralCircle
@ SpiralCircle
Definition: nav_spiral.h:35
SpiralInc
@ SpiralInc
Definition: nav_spiral.h:35
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
NavSpiral::segments
float segments
Definition: nav_spiral.h:44
dc_stop
uint8_t dc_stop(void)
Stop dc control.
Definition: dc.c:256
nav_circle_XY
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Definition: nav.c:109
NavSpiral::center
struct FloatVect3 center
Definition: nav_spiral.h:41
stateGetPositionUtm_f
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
waypoints
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
FLOAT_VECT3_NORM
#define FLOAT_VECT3_NORM(_v)
Definition: pprz_algebra_float.h:164
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
dc.h
NavVerticalAutoThrottleMode
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition: nav.h:171
VECT2_DIFF
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
VECT2_COPY
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
pos_diff
struct FloatVect2 pos_diff
Definition: obstacle_avoidance.c:75
FloatVect2
Definition: pprz_algebra_float.h:49
nav_route_xy
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:381
float_vect2_norm
static float float_vect2_norm(struct FloatVect2 *v)
Definition: pprz_algebra_float.h:139
NavSpiral::last_circle
struct FloatVect2 last_circle
Definition: nav_spiral.h:40
UtmCoor_f::alt
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
Definition: pprz_geodetic_float.h:84
dc_send_command
void dc_send_command(uint8_t cmd)
Send Command To Camera.
Definition: atmega_i2c_cam_ctrl.c:41
NavSpiral::dist_from_center
float dist_from_center
Definition: nav_spiral.h:42
NavSpiral::radius_start
float radius_start
Definition: nav_spiral.h:47
nav_spiral.h
uint8_t
unsigned char uint8_t
Definition: types.h:14
NAV_SPIRAL_MIN_CIRCLE_RADIUS
#define NAV_SPIRAL_MIN_CIRCLE_RADIUS
Definition: nav_spiral.c:45
EnuCoor_f
vector in East North Up coordinates Units: meters
Definition: pprz_geodetic_float.h:72
NavSpiral::radius_min
float radius_min
Definition: nav_spiral.h:46
autopilot.h
NavSpiral::trans_current
struct FloatVect3 trans_current
Definition: nav_spiral.h:38
nav_spiral_run
bool nav_spiral_run(void)
Definition: nav_spiral.c:86
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
NavSpiral::status
enum SpiralStatus status
Definition: nav_spiral.h:49
nav.h
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
nav_approaching_xy
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:323
DC_SHOOT
@ DC_SHOOT
Definition: dc.h:100
nav_spiral_setup
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
dc_cam_angle
float dc_cam_angle
camera angle
Definition: dc.c:70
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
EnuCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:73
state.h
NavSpiral
Definition: nav_spiral.h:37
NavVerticalAltitudeMode
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition: nav.h:185