Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
32
34
36#include "state.h"
37#include "autopilot.h"
38#include "generated/flight_plan.h"
39
40#ifdef DIGITAL_CAM
42#endif
43
44#ifndef NAV_SPIRAL_MIN_CIRCLE_RADIUS
45#define NAV_SPIRAL_MIN_CIRCLE_RADIUS 60
46#endif
47
49
50void 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
55 nav_spiral.segments = segments;
57 if (nav_spiral.radius_start < nav_spiral.radius_min) {
58 nav_spiral.radius_start = nav_spiral.radius_min;
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
65 nav_spiral.radius = float_vect2_norm(&edge);
66
67 // get a copy of the current position
68 struct EnuCoor_f pos_enu = *stateGetPositionEnu_f();
69
70 VECT2_DIFF(nav_spiral.trans_current, pos_enu, nav_spiral.center);
71 nav_spiral.trans_current.z = stateGetPositionUtm_f()->alt - nav_spiral.center.z;
72
73 nav_spiral.dist_from_center = FLOAT_VECT3_NORM(nav_spiral.trans_current);
74
75 // nav_spiral.alpha_limit denotes angle, where the radius will be increased
76 nav_spiral.alpha_limit = 2 * M_PI / nav_spiral.segments;
77 //current position
78 nav_spiral.fly_from.x = stateGetPositionEnu_f()->x;
79 nav_spiral.fly_from.y = stateGetPositionEnu_f()->y;
80
81 if (nav_spiral.dist_from_center > nav_spiral.radius) {
83 }
84}
85
87{
88 struct EnuCoor_f pos_enu = *stateGetPositionEnu_f();
89
90 VECT2_DIFF(nav_spiral.trans_current, pos_enu, nav_spiral.center);
91 nav_spiral.dist_from_center = FLOAT_VECT3_NORM(nav_spiral.trans_current);
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
99 nav_route_xy(nav_spiral.fly_from.x, nav_spiral.fly_from.y, nav_spiral.center.x, nav_spiral.center.y);
100 // center reached?
101 if (nav_approaching_xy(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.fly_from.x, nav_spiral.fly_from.y, 0)) {
102 // nadir image
103#ifdef DIGITAL_CAM
105#endif
107 }
108 break;
110 // Starts helix
111 // storage of current coordinates
112 // calculation needed, State switch to SpiralCircle
113 nav_circle_XY(nav_spiral.center.y, nav_spiral.center.y, nav_spiral.radius_start);
114 if (nav_spiral.dist_from_center >= nav_spiral.radius_start) {
115 VECT2_COPY(nav_spiral.last_circle, pos_enu);
116 nav_spiral.status = SpiralCircle;
117 // Start helix
118#ifdef DIGITAL_CAM
119 dc_Circle(360 / nav_spiral.segments);
120#endif
121 }
122 break;
123 case SpiralCircle: {
124 nav_circle_XY(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.radius_start);
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) {
136 VECT2_COPY(nav_spiral.last_circle, pos_enu);
137 nav_spiral.status = SpiralInc;
138 }
139 break;
140 }
141 case SpiralInc:
142 // increasing circle radius as long as it is smaller than max helix radius
143 if (nav_spiral.radius_start + nav_spiral.radius_increment < nav_spiral.radius) {
144 nav_spiral.radius_start = nav_spiral.radius_start + nav_spiral.radius_increment;
145#ifdef DIGITAL_CAM
146 if (dc_cam_tracing) {
147 // calculating Cam angle for camera alignment
148 nav_spiral.trans_current.z = stateGetPositionUtm_f()->alt - nav_spiral.center.z;
149 dc_cam_angle = atan(nav_spiral.radius_start / nav_spiral.trans_current.z) * 180 / M_PI;
150 }
151#endif
152 } else {
153 nav_spiral.radius_start = nav_spiral.radius;
154#ifdef DIGITAL_CAM
155 // Stopps DC
156 dc_stop();
157#endif
158 }
159 nav_spiral.status = SpiralCircle;
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:44
uint8_t dc_stop(void)
Stop dc control.
Definition dc.c:262
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:196
@ DC_SHOOT
Definition dc.h:102
#define FLOAT_VECT3_NORM(_v)
static float float_vect2_norm(const struct FloatVect2 *v)
#define VECT2_DIFF(_c, _a, _b)
#define VECT2_COPY(_a, _b)
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition state.h:832
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:859
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.
@ 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
float segments
Definition nav_spiral.h:44
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.