Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
nav_spiral_3D.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
29 
31 #include "state.h"
32 #include "autopilot.h"
33 #include "generated/flight_plan.h"
34 
35 #ifndef NAV_SPIRAL_3D_DIST_DIFF
36 #define NAV_SPIRAL_3D_DIST_DIFF 10.f // horizontal distance difference before starting
37 #endif
38 
39 #ifndef NAV_SPIRAL_3D_ALT_DIFF
40 #define NAV_SPIRAL_3D_ALT_DIFF 10.f // vertical distance difference before starting
41 #endif
42 
43 #ifndef NAV_SPIRAL_3D_MIN_CIRCLE_RADIUS
44 #define NAV_SPIRAL_3D_MIN_CIRCLE_RADIUS 50.f
45 #endif
46 
48 
49 struct NavSpiral3D {
52  float alt_start;
53  float alt_stop;
54  float radius;
55  float radius_min;
56  float radius_start;
57  float radius_stop;
60 };
61 
63 
64 static const float nav_dt = 1.f / NAVIGATION_FREQUENCY;
65 
66 #if USE_MISSION
68 
69 static bool nav_spiral_3D_mission(uint8_t nb, float *params, enum MissionRunFlag flag)
70 {
71  if (flag == MissionInit && nb == 9) {
72  float cx = params[0];
73  float cy = params[1];
74  float alt_start = params[2];
75  float alt_stop = params[3];
76  float r_start = params[4];
77  float r_stop = params[5];
78  float vx = params[6];
79  float vy = params[7];
80  float vz = params[8];
81  nav_spiral_3D_setup(cx, cy, alt_start, alt_stop, r_start, r_stop, vx, vy, vz);
82  return true;
83  }
84  else if (flag == MissionUpdate && nb == 4) {
85  // update current position and horizontal speed
86  float cx = params[0];
87  float cy = params[1];
88  float vx = params[2];
89  float vy = params[3];
90  nav_spiral_3D.center.x = cx;
91  nav_spiral_3D.center.y = cy;
94  return true;
95  }
96  else if (flag == MissionUpdate && nb == 2) {
97  // update horizontal speed
98  float vx = params[0];
99  float vy = params[1];
102  return true;
103  }
104  else if (flag == MissionUpdate && nb == 1) {
105  // update vertical speed
106  float vz = params[0];
108  return true;
109  }
110  else if (flag == MissionRun) {
111  return nav_spiral_3D_run();
112  }
113  return false; // not a valid case
114 }
115 #endif
116 
118 {
119  // no speed by default
121 
122 #if USE_MISSION
123  mission_register(nav_spiral_3D_mission, "SPIR3");
124 #endif
125 }
126 
127 void nav_spiral_3D_setup(float center_x, float center_y,
128  float alt_start, float alt_stop,
129  float radius_start, float radius_stop,
130  float vx, float vy, float vz)
131 {
132  // initial center position
133  VECT3_ASSIGN(nav_spiral_3D.center, center_x, center_y, alt_start);
136  float deltaZ = alt_stop - alt_start;
137  if (deltaZ * vz < 0.f &&
138  fabsf(deltaZ) > 1.f &&
139  fabsf(radius_start - radius_stop) > 0.1f) {
140  // error vz and delta Z don't have the same sign
141  // and we are not doing a circle at constant alt
143  return;
144  }
145  // increment based on speed
147  // radius
152  }
157  }
158  // Compute radius increment
159  float deltaR = radius_stop - radius_start;
160  if (fabsf(deltaR) < 1.f) {
161  // radius diff is too small we are doing a circle of constant radius
163  } else if (fabsf(deltaZ) < 1.f && fabsf(vz) > 0.1f) {
164  // alt diff is too small, use Vz as increment rate at fix altitude
165  // Rinc = deltaR / deltaT
166  // deltaT = deltaR / Vz
167  // Rinc = Vz
168  float sign = deltaR < 0.f ? -1.f : 1.0;
169  nav_spiral_3D.pos_incr.z = 0.f;
170  nav_spiral_3D.radius_increment = sign * fabsf(vz) * nav_dt;
171  } else if (fabsf(vz) < 0.1f) {
172  // vz is too small, fail
174  return;
175  } else {
176  // normal case, vz and alt diff are large enough and in the same direction
177  // Rinc = deltaR / deltaT
178  // deltaT = deltaZ / Vz
179  // Rinc = deltaR * Vz / deltaZ;
180  nav_spiral_3D.radius_increment = deltaR * vz * nav_dt / deltaZ;
181  }
183 }
184 
186 {
187  struct EnuCoor_f pos_enu = *stateGetPositionEnu_f();
188 
189  struct FloatVect2 pos_diff;
191  float dist_to_center = float_vect2_norm(&pos_diff);
192  float dist_diff, alt_diff;
193  float pre_climb = 0.f;
194 
195  switch (nav_spiral_3D.status) {
196  case Spiral3DFail:
197  // error during setup
198  return false;
199 
200  case Spiral3DStart:
201  // fly to start circle until dist to center and alt are acceptable
202  // flys until center of the helix is reached and start helix
204  dist_diff = fabs(dist_to_center - nav_spiral_3D.radius_start);
205  alt_diff = fabs(stateGetPositionUtm_f()->alt - nav_spiral_3D.alt_start);
206  if (dist_diff < NAV_SPIRAL_3D_DIST_DIFF && alt_diff < NAV_SPIRAL_3D_ALT_DIFF) {
208  }
209  break;
210 
211  case Spiral3DCircle:
212  // increment center position
214  if (fabsf(nav_spiral_3D.radius_increment) > 0.001f) {
215  // increment radius
217  // test end condition
218  dist_diff = fabs(dist_to_center - nav_spiral_3D.radius_stop);
219  alt_diff = fabs(stateGetPositionUtm_f()->alt - nav_spiral_3D.alt_stop);
220  if (dist_diff < NAV_SPIRAL_3D_DIST_DIFF && alt_diff < NAV_SPIRAL_3D_ALT_DIFF) {
221  // reaching desired altitude and radius
222  return false; // spiral is finished
223  }
224  } else {
225  // we are doing a circle of constant radius
226  if (fabsf(nav_spiral_3D.pos_incr.z) > 0.1f) {
227  // alt should change, check for arrival
228  alt_diff = fabs(stateGetPositionUtm_f()->alt - nav_spiral_3D.alt_stop);
229  if (alt_diff < NAV_SPIRAL_3D_ALT_DIFF) {
230  // reaching desired altitude
231  return false; // spiral is finished
232  }
233  }
234  }
235  pre_climb = nav_spiral_3D.pos_incr.z / nav_dt;
236  break;
237 
238  default:
239  break;
240  }
241 
242  // horizontal control setting
244  // vertical control setting
245  NavVerticalAutoThrottleMode(0.); /* No pitch */
246  NavVerticalAltitudeMode(nav_spiral_3D.center.z, pre_climb); /* No preclimb */
247 
248  return true;
249 }
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
NAV_SPIRAL_3D_ALT_DIFF
#define NAV_SPIRAL_3D_ALT_DIFF
Definition: nav_spiral_3D.c:40
NavSpiral3D::radius_min
float radius_min
Definition: nav_spiral_3D.c:55
NavSpiral3D::radius_start
float radius_start
Definition: nav_spiral_3D.c:56
Spiral3DFail
@ Spiral3DFail
Definition: nav_spiral_3D.c:47
NavSpiral3D::radius_increment
float radius_increment
Definition: nav_spiral_3D.c:58
NavSpiral3D::pos_incr
struct FloatVect3 pos_incr
Definition: nav_spiral_3D.c:51
NAV_SPIRAL_3D_MIN_CIRCLE_RADIUS
#define NAV_SPIRAL_3D_MIN_CIRCLE_RADIUS
Definition: nav_spiral_3D.c:44
MissionInit
@ MissionInit
first exec
Definition: mission_common.h:55
NavSpiral3D::radius
float radius
Definition: nav_spiral_3D.c:54
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
nav_circle_XY
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Definition: nav.c:111
Spiral3DStatus
Spiral3DStatus
Definition: nav_spiral_3D.c:47
stateGetPositionUtm_f
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
nav_dt
static const float nav_dt
Definition: nav_spiral_3D.c:64
nav_spiral_3D_setup
void nav_spiral_3D_setup(float center_x, float center_y, float alt_start, float alt_stop, float radius_start, float radius_stop, float vx, float vy, float vz)
Initialize spiral 3D based on:
Definition: nav_spiral_3D.c:127
VECT3_ADD
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
nav_spiral_3D_init
void nav_spiral_3D_init(void)
Definition: nav_spiral_3D.c:117
NavVerticalAutoThrottleMode
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition: nav.h:180
VECT2_DIFF
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
pos_diff
struct FloatVect2 pos_diff
Definition: obstacle_avoidance.c:75
FloatVect2
Definition: pprz_algebra_float.h:49
FloatVect3
Definition: pprz_algebra_float.h:54
float_vect2_norm
static float float_vect2_norm(struct FloatVect2 *v)
Definition: pprz_algebra_float.h:139
sign
static float sign(float x)
sign function
Definition: nav_fish.c:232
Spiral3DStart
@ Spiral3DStart
Definition: nav_spiral_3D.c:47
FLOAT_VECT3_ZERO
#define FLOAT_VECT3_ZERO(_v)
Definition: pprz_algebra_float.h:161
NavSpiral3D::radius_stop
float radius_stop
Definition: nav_spiral_3D.c:57
Spiral3DCircle
@ Spiral3DCircle
Definition: nav_spiral_3D.c:47
NavSpiral3D::status
enum Spiral3DStatus status
Definition: nav_spiral_3D.c:59
NavSpiral3D::alt_start
float alt_start
Definition: nav_spiral_3D.c:52
NAVIGATION_FREQUENCY
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
Definition: nav.h:49
nav_spiral_3D.h
nav_spiral_3D
struct NavSpiral3D nav_spiral_3D
Definition: nav_spiral_3D.c:62
MissionRun
@ MissionRun
normal run
Definition: mission_common.h:54
EnuCoor_f
vector in East North Up coordinates Units: meters
Definition: pprz_geodetic_float.h:72
NavSpiral3D::alt_stop
float alt_stop
Definition: nav_spiral_3D.c:53
autopilot.h
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
nav.h
mission_common.h
mission planner library
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
MissionUpdate
@ MissionUpdate
param update
Definition: mission_common.h:56
mission_register
bool mission_register(mission_custom_cb cb, char *type)
Register a new navigation or action callback function.
Definition: mission_common.c:115
NavSpiral3D
Definition: nav_spiral_3D.c:49
NAV_SPIRAL_3D_DIST_DIFF
#define NAV_SPIRAL_3D_DIST_DIFF
Definition: nav_spiral_3D.c:36
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
state.h
VECT3_ASSIGN
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
MissionRunFlag
MissionRunFlag
Definition: mission_common.h:53
NavSpiral3D::center
struct FloatVect3 center
Definition: nav_spiral_3D.c:50
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:194
nav_spiral_3D_run
bool nav_spiral_3D_run(void)
Run spiral 3D navigation.
Definition: nav_spiral_3D.c:185