Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
27
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
61
63
64static const float nav_dt = 1.f / NAVIGATION_FREQUENCY;
65
66#if USE_MISSION
68
69static 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;
92 nav_spiral_3D.pos_incr.x = vx * nav_dt;
93 nav_spiral_3D.pos_incr.y = vy * nav_dt;
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];
100 nav_spiral_3D.pos_incr.x = vx * nav_dt;
101 nav_spiral_3D.pos_incr.y = vy * nav_dt;
102 return true;
103 }
104 else if (flag == MissionUpdate && nb == 1) {
105 // update vertical speed
106 float vz = params[0];
107 nav_spiral_3D.pos_incr.z = vz * nav_dt;
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
127void 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);
134 nav_spiral_3D.alt_start = alt_start;
135 nav_spiral_3D.alt_stop = alt_stop;
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
146 VECT3_ASSIGN(nav_spiral_3D.pos_incr, vx*nav_dt, vy*nav_dt, vz*nav_dt);
147 // radius
149 nav_spiral_3D.radius_start = radius_start;
150 if (nav_spiral_3D.radius_start < nav_spiral_3D.radius_min) {
151 nav_spiral_3D.radius_start = nav_spiral_3D.radius_min;
152 }
153 nav_spiral_3D.radius = nav_spiral_3D.radius_start; // start radius
154 nav_spiral_3D.radius_stop = radius_stop;
155 if (nav_spiral_3D.radius_stop < nav_spiral_3D.radius_min) {
156 nav_spiral_3D.radius_stop = nav_spiral_3D.radius_min;
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
162 nav_spiral_3D.radius_increment = 0.f;
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;
190 VECT2_DIFF(pos_diff, pos_enu, nav_spiral_3D.center);
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
203 VECT2_DIFF(pos_diff, pos_enu, nav_spiral_3D.center);
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
213 VECT3_ADD(nav_spiral_3D.center, nav_spiral_3D.pos_incr);
214 if (fabsf(nav_spiral_3D.radius_increment) > 0.001f) {
215 // increment radius
216 nav_spiral_3D.radius += nav_spiral_3D.radius_increment;
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
243 nav_circle_XY(nav_spiral_3D.center.x, nav_spiral_3D.center.y, nav_spiral_3D.radius);
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}
Core autopilot interface common to all firmwares.
#define FLOAT_VECT3_ZERO(_v)
static float float_vect2_norm(const struct FloatVect2 *v)
#define VECT2_DIFF(_c, _a, _b)
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define VECT3_ADD(_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 mission_register(mission_custom_cb cb, char *type)
Register a new navigation or action callback function.
mission planner library
MissionRunFlag
@ MissionInit
first exec
@ MissionRun
normal run
@ MissionUpdate
param update
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Definition nav.c:108
Fixedwing Navigation library.
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
Definition nav.h:49
#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
static float sign(float x)
sign function
Definition nav_fish.c:232
Spiral3DStatus
@ Spiral3DStart
@ Spiral3DFail
@ Spiral3DCircle
void nav_spiral_3D_init(void)
#define NAV_SPIRAL_3D_MIN_CIRCLE_RADIUS
#define NAV_SPIRAL_3D_ALT_DIFF
struct FloatVect3 center
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:
#define NAV_SPIRAL_3D_DIST_DIFF
float radius_start
float radius_stop
float radius_increment
bool nav_spiral_3D_run(void)
Run spiral 3D navigation.
struct NavSpiral3D nav_spiral_3D
struct FloatVect3 pos_incr
enum Spiral3DStatus status
Fixedwing navigation in a 3D spiral.
static const float nav_dt
struct FloatVect2 pos_diff
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.