Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
nav_rotorcraft_hybrid.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2023 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
25
28#include "math/pprz_isa.h"
29#include "generated/flight_plan.h"
30
31// if NAV_HYBRID_MAX_BANK is not defined, set it to 30 degrees.
32#ifndef NAV_HYBRID_MAX_BANK
33float nav_hybrid_max_bank = 0.52f;
34#else
35float nav_hybrid_max_bank = NAV_HYBRID_MAX_BANK;
36#endif
37
38// Max ground speed that will be commanded
39#ifndef NAV_HYBRID_MAX_AIRSPEED
40#define NAV_HYBRID_MAX_AIRSPEED 15.0f
41#endif
42
43#ifndef NAV_HYBRID_SPEED_MARGIN
44#define NAV_HYBRID_SPEED_MARGIN 10.0f
45#endif
46
47#define NAV_MAX_SPEED (NAV_HYBRID_MAX_AIRSPEED + NAV_HYBRID_SPEED_MARGIN)
49
50#ifndef NAV_HYBRID_GOTO_MAX_SPEED
51#define NAV_HYBRID_GOTO_MAX_SPEED NAV_MAX_SPEED
52#endif
53
54#ifndef NAV_HYBRID_MAX_DECELERATION
55#define NAV_HYBRID_MAX_DECELERATION 1.0
56#endif
57
58#ifndef NAV_HYBRID_MAX_ACCELERATION
59#define NAV_HYBRID_MAX_ACCELERATION 4.0
60#endif
61
62#ifndef NAV_HYBRID_SOFT_ACCELERATION
63#define NAV_HYBRID_SOFT_ACCELERATION NAV_HYBRID_MAX_ACCELERATION
64#endif
65
66float nav_max_deceleration_sp = NAV_HYBRID_MAX_DECELERATION; //Maximum deceleration allowed for the set-point
67float nav_max_acceleration_sp = NAV_HYBRID_MAX_ACCELERATION; //Maximum acceleration allowed for the set-point
68float nav_hybrid_soft_acceleration = NAV_HYBRID_SOFT_ACCELERATION; //Soft acceleration limit allowed for the set-point (Equal to the maximum acceleration by default)
69float nav_hybrid_max_acceleration = NAV_HYBRID_MAX_ACCELERATION; //Maximum general limit in acceleration allowed for the hybrid navigation
70
71#ifndef NAV_HYBRID_MAX_EXPECTED_WIND
72#define NAV_HYBRID_MAX_EXPECTED_WIND 5.0f
73#endif
75
76#ifdef NAV_HYBRID_LINE_GAIN
77float nav_hybrid_line_gain = NAV_HYBRID_LINE_GAIN;
78#else
80#endif
81
82#ifndef NAV_HYBRID_NAV_LINE_DIST
83#define NAV_HYBRID_NAV_LINE_DIST 50.f
84#endif
85
86#ifndef NAV_HYBRID_NAV_CIRCLE_DIST
87#define NAV_HYBRID_NAV_CIRCLE_DIST 40.f
88#endif
89
90#ifdef GUIDANCE_INDI_POS_GAIN
92#elif defined(NAV_HYBRID_POS_GAIN)
93float nav_hybrid_pos_gain = NAV_HYBRID_POS_GAIN;
94#else
96#endif
97
98#if defined(NAV_HYBRID_POS_GAIN) && defined(GUIDANCE_INDI_POS_GAIN)
99#warning "NAV_HYBRID_POS_GAIN and GUIDANCE_INDI_POS_GAIN serve similar purpose and are both defined, using GUIDANCE_INDI_POS_GAIN"
100#endif
101
102#ifndef GUIDANCE_INDI_HYBRID
103bool force_forward = 0.0f;
104#endif
105
106/* When using external vision, run the nav functions in position mode for better tracking*/
107#ifndef NAV_HYBRID_EXT_VISION_SETPOINT_MODE
108#define NAV_HYBRID_EXT_VISION_SETPOINT_MODE FALSE
109#endif
110
111/* Use max target groundspeed and max acceleration to limit circle radius*/
112#ifndef NAV_HYBRID_LIMIT_CIRCLE_RADIUS
113#define NAV_HYBRID_LIMIT_CIRCLE_RADIUS FALSE
114#endif
115
116static float max_speed_for_deceleration(float dist_to_wp);
117
120
121static void nav_hybrid_goto(struct EnuCoor_f *wp)
122{
124 nav_rotorcraft_base.goto_wp.to = *wp;
125 nav_rotorcraft_base.goto_wp.dist2_to_wp = get_dist2_to_point(wp);
126 VECT2_COPY(nav.target, *wp);
127
128 // Calculate position error
129 struct FloatVect2 pos_error;
130 struct EnuCoor_f *pos = stateGetPositionEnu_f();
131 VECT2_DIFF(pos_error, nav.target, *pos);
132
133 struct FloatVect2 speed_sp;
135
136 // Bound the setpoint velocity vector
137 float max_h_speed = nav_max_speed;
138 if (!force_forward) {
139 // If not in force_forward, compute speed based on deceleration and nav_max_speed
140 // Calculate distance to waypoint
141 float dist_to_wp = float_vect2_norm(&pos_error);
142 max_h_speed = max_speed_for_deceleration(dist_to_wp);
143 }
144 float_vect2_bound_in_2d(&speed_sp, max_h_speed);
145
146 VECT2_COPY(nav.speed, speed_sp);
147 nav.horizontal_mode = NAV_HORIZONTAL_MODE_WAYPOINT;
148
149 // In optitrack tests use position mode for more precise hovering
150#if NAV_HYBRID_EXT_VISION_SETPOINT_MODE
151 nav.setpoint_mode = NAV_SETPOINT_MODE_POS;
152#else
153 nav.setpoint_mode = NAV_SETPOINT_MODE_SPEED;
154#endif
155}
156
157static void nav_hybrid_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
158{
159 struct FloatVect2 wp_diff, pos_diff;
160 VECT2_DIFF(wp_diff, *wp_end, *wp_start);
162
163 // Calculate magnitude of the desired speed vector based on distance to waypoint
164 float dist_to_target = float_vect2_norm(&pos_diff);
165 float desired_speed;
166 if (force_forward) {
167 desired_speed = nav_max_speed;
168 } else {
169 desired_speed = dist_to_target * nav_hybrid_pos_gain;
170 float max_h_speed = max_speed_for_deceleration(dist_to_target);
171 Bound(desired_speed, 0.0f, max_h_speed);
172 }
173
174 // Calculate length of line segment
175 float length_line = Max(float_vect2_norm(&wp_diff), 0.01f);
176 // Normal vector to the line, with length of the line
177 struct FloatVect2 normalv;
178 VECT2_ASSIGN(normalv, -wp_diff.y, wp_diff.x);
179 // Length of normal vector is the same as of the line segment
180 float length_normalv = length_line; // >= 0.01
181 // Distance along the normal vector
182 float dist_to_line = (pos_diff.x * normalv.x + pos_diff.y * normalv.y) / length_normalv;
183 // Normal vector scaled to be the distance to the line
184 struct FloatVect2 v_to_line, v_along_line;
185 v_to_line.x = dist_to_line * normalv.x / length_normalv * nav_hybrid_line_gain;
186 v_to_line.y = dist_to_line * normalv.y / length_normalv * nav_hybrid_line_gain;
187 // The distance that needs to be traveled along the line
188 v_along_line.x = wp_diff.x / length_line * NAV_HYBRID_NAV_LINE_DIST;
189 v_along_line.y = wp_diff.y / length_line * NAV_HYBRID_NAV_LINE_DIST;
190 // Calculate the desired direction to converge to the line
191 struct FloatVect2 direction;
192 VECT2_SMUL(direction, v_along_line, (1.f / (1.f + fabsf(dist_to_line) * 0.05f)));
193 VECT2_ADD(direction, v_to_line);
194 float length_direction = Max(float_vect2_norm(&direction), 0.01f);
195 // Scale to have the desired speed
196 VECT2_SMUL(nav.speed, direction, desired_speed / length_direction);
197 // final target position, should be on the line, for display
199
200 nav_rotorcraft_base.goto_wp.from = *wp_start;
201 nav_rotorcraft_base.goto_wp.to = *wp_end;
202 nav_rotorcraft_base.goto_wp.dist2_to_wp = get_dist2_to_point(wp_end);
203 nav.horizontal_mode = NAV_HORIZONTAL_MODE_ROUTE;
204 nav.setpoint_mode = NAV_SETPOINT_MODE_SPEED;
205}
206
214static float max_speed_for_deceleration(float dist_to_wp) {
215 float max_speed_decel2 = fabsf(2.f * dist_to_wp * nav_max_deceleration_sp); // dist_to_wp can only be positive, but just in case
216 float max_speed_decel = sqrtf(max_speed_decel2);
217 // Bound the setpoint velocity vector
218 float max_h_speed = Min(nav_max_speed, max_speed_decel);
219 return max_h_speed;
220}
221
222static bool nav_hybrid_approaching(struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time)
223{
224 float dist_to_point;
225 struct FloatVect2 diff;
226 struct EnuCoor_f *pos = stateGetPositionEnu_f();
227
228 /* if an approaching_time is given, estimate diff after approching_time secs */
229 if (approaching_time > 0.f) {
230 struct FloatVect2 estimated_pos;
231 struct FloatVect2 estimated_progress;
232 struct EnuCoor_f *speed = stateGetSpeedEnu_f();
233 VECT2_SMUL(estimated_progress, *speed, approaching_time);
234 VECT2_SUM(estimated_pos, *pos, estimated_progress);
235 VECT2_DIFF(diff, *wp, estimated_pos);
236 }
237 /* else use current position */
238 else {
239 VECT2_DIFF(diff, *wp, *pos);
240 }
241 /* compute distance of estimated/current pos to target wp
242 */
243 dist_to_point = float_vect2_norm(&diff);
244
245 /* return TRUE if we have arrived */
246 if (dist_to_point < ARRIVED_AT_WAYPOINT) {
247 return true;
248 }
249
250 /* if coming from a valid waypoint */
251 if (from != NULL) {
252 /* return TRUE if normal line at the end of the segment is crossed */
253 struct FloatVect2 from_diff;
254 VECT2_DIFF(from_diff, *wp, *from);
255 return (diff.x * from_diff.x + diff.y * from_diff.y < 0.f);
256 }
257
258 return false;
259}
260
261static void nav_hybrid_circle(struct EnuCoor_f *wp_center, float radius)
262{
263 struct FloatVect2 pos_diff;
264 float desired_speed;
265
267 // direction of rotation
268 float sign_radius = (radius > 0.f) ? 1.f : (radius < 0.f) ? -1.f : 0.f;
269 // absolute radius
270 float abs_radius = fabsf(radius);
271#if NAV_HYBRID_LIMIT_CIRCLE_RADIUS
273 abs_radius = Max(abs_radius, min_radius);
274#endif
275 if (abs_radius > 0.1f) {
276 // store last qdr
277 float last_qdr = nav_rotorcraft_base.circle.qdr;
278 // compute qdr
279 nav_rotorcraft_base.circle.qdr = atan2f(pos_diff.y, pos_diff.x);
280 // increment circle radians
281 float trigo_diff = nav_rotorcraft_base.circle.qdr - last_qdr;
282 NormRadAngle(trigo_diff);
283 nav_rotorcraft_base.circle.radians += trigo_diff;
284 // progress angle
285 float progress_angle = NAV_HYBRID_NAV_CIRCLE_DIST / abs_radius;
286 Bound(progress_angle, M_PI / 16.f, M_PI / 4.f);
287 float alpha = nav_rotorcraft_base.circle.qdr - sign_radius * progress_angle;
288 // final target position, should be on the circle, for display
289 nav.target.x = wp_center->x + cosf(alpha) * abs_radius;
290 nav.target.y = wp_center->y + sinf(alpha) * abs_radius;
291 }
292 else {
293 // radius is too small, direct to center
294 VECT2_COPY(nav.target, *wp_center);
295 }
296 // compute desired speed
297 float radius_diff = fabsf(float_vect2_norm(&pos_diff) - abs_radius);
298 if (radius_diff > NAV_HYBRID_NAV_CIRCLE_DIST) {
299 // far from circle, speed proportional to diff
300 desired_speed = radius_diff * nav_hybrid_pos_gain;
302 } else {
303 // close to circle, speed function of radius for a feasible turn
304 // 0.8 * MAX_BANK gives some margins for the turns
305 desired_speed = sqrtf(PPRZ_ISA_GRAVITY * abs_radius * tanf(0.8f * nav_hybrid_max_bank));
307 }
308 if (force_forward) {
309 desired_speed = nav_max_speed;
311 }
312 Bound(desired_speed, 0.0f, nav_max_speed);
313 // compute speed vector from target position
314 struct FloatVect2 speed_unit;
315 VECT2_DIFF(speed_unit, nav.target, *stateGetPositionEnu_f());
316 float_vect2_normalize(&speed_unit);
317 VECT2_SMUL(nav.speed, speed_unit, desired_speed);
318 nav_rotorcraft_base.circle.center = *wp_center;
319 nav_rotorcraft_base.circle.radius = sign_radius * abs_radius;
320 nav.horizontal_mode = NAV_HORIZONTAL_MODE_CIRCLE;
321 nav.setpoint_mode = NAV_SETPOINT_MODE_SPEED;
322}
323
331{
333 nav_rotorcraft_base.goto_wp.leg_progress = 0.f;
334 nav_rotorcraft_base.goto_wp.leg_length = 1.f;
335
336 // register nav functions
339}
340
341
#define Min(x, y)
Definition esc_dshot.c:110
static void float_vect2_normalize(struct FloatVect2 *v)
normalize 2D vector in place
static float float_vect2_norm(const struct FloatVect2 *v)
void float_vect2_bound_in_2d(struct FloatVect2 *vect2, const float bound)
#define VECT2_ADD(_a, _b)
#define VECT2_SMUL(_vo, _vi, _s)
#define VECT2_DIFF(_c, _a, _b)
#define VECT2_COPY(_a, _b)
#define VECT2_SUM(_c, _a, _b)
#define VECT2_ASSIGN(_a, _x, _y)
#define PPRZ_ISA_GRAVITY
earth-surface gravitational acceleration in m/s^2
Definition pprz_isa.h:56
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:859
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition state.h:1069
struct FloatVect3 speed_sp
bool force_forward
forward flight for hybrid nav
#define GUIDANCE_INDI_POS_GAIN
struct NavBase_t nav_rotorcraft_base
Basic Nav struct.
float nav_max_deceleration_sp
static void nav_hybrid_circle(struct EnuCoor_f *wp_center, float radius)
#define NAV_HYBRID_MAX_ACCELERATION
static void nav_hybrid_goto(struct EnuCoor_f *wp)
Implement basic nav function for the hybrid case.
#define NAV_HYBRID_SOFT_ACCELERATION
#define NAV_HYBRID_NAV_LINE_DIST
#define NAV_HYBRID_NAV_CIRCLE_DIST
static void nav_hybrid_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
float nav_max_speed
float nav_hybrid_max_bank
float nav_max_acceleration_sp
#define NAV_HYBRID_MAX_EXPECTED_WIND
float nav_hybrid_max_expected_wind
float nav_hybrid_line_gain
float nav_hybrid_pos_gain
static bool nav_hybrid_approaching(struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time)
static float max_speed_for_deceleration(float dist_to_wp)
Calculate max speed when decelerating at MAX capacity a_max distance travelled d = 1/2 a_max t^2 The ...
void nav_rotorcraft_hybrid_init(void)
Init and register nav functions.
#define NAV_HYBRID_MAX_DECELERATION
float nav_hybrid_max_acceleration
float nav_hybrid_soft_acceleration
#define NAV_MAX_SPEED
Specific navigation functions for hybrid aircraft.
struct FloatVect2 pos_diff
uint8_t direction
float y
in meters
float x
in meters
vector in East North Up coordinates Units: meters
Paparazzi atmospheric pressure conversion utilities.
struct RotorcraftNavigation nav
Definition navigation.c:51
void nav_register_circle(navigation_circle nav_circle)
Definition navigation.c:399
void nav_register_goto_wp(navigation_goto nav_goto, navigation_route nav_route, navigation_approaching nav_approaching)
Definition navigation.c:392
float get_dist2_to_point(struct EnuCoor_f *p)
Returns squared horizontal distance to given point.
Definition navigation.c:308
Rotorcraft navigation functions.
#define NAV_HORIZONTAL_MODE_WAYPOINT
Nav modes these modes correspond to the flight plan instructions used to set the high level navigatio...
Definition navigation.h:85
#define NAV_HORIZONTAL_MODE_CIRCLE
Definition navigation.h:87
#define NAV_HORIZONTAL_MODE_ROUTE
Definition navigation.h:86
#define NAV_SETPOINT_MODE_SPEED
Definition navigation.h:101
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
Definition navigation.h:42
#define NAV_SETPOINT_MODE_POS
Nav setpoint modes these modes correspond to submodes defined by navigation routines to tell which se...
Definition navigation.h:100
#define ARRIVED_AT_WAYPOINT
minimum horizontal distance to waypoint to mark as arrived
Definition navigation.h:55
float alpha
Definition textons.c:133