Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
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
33 #define NAV_HYBRID_MAX_BANK 0.52f
34 #endif
35 
36 // Max ground speed that will be commanded
37 #ifndef NAV_HYBRID_MAX_AIRSPEED
38 #define NAV_HYBRID_MAX_AIRSPEED 15.0f
39 #endif
40 
41 #ifndef NAV_HYBRID_SPEED_MARGIN
42 #define NAV_HYBRID_SPEED_MARGIN 10.0f
43 #endif
44 
45 #define NAV_MAX_SPEED (NAV_HYBRID_MAX_AIRSPEED + NAV_HYBRID_SPEED_MARGIN)
47 
48 #ifndef NAV_HYBRID_GOTO_MAX_SPEED
49 #define NAV_HYBRID_GOTO_MAX_SPEED NAV_MAX_SPEED
50 #endif
51 
53 
54 #ifndef NAV_HYBRID_MAX_DECELERATION
55 #define NAV_HYBRID_MAX_DECELERATION 1.0
56 #endif
57 
59 
60 #ifdef NAV_HYBRID_LINE_GAIN
61 float nav_hybrid_line_gain = NAV_HYBRID_LINE_GAIN;
62 #else
63 float nav_hybrid_line_gain = 1.0f;
64 #endif
65 
66 #ifndef NAV_HYBRID_NAV_LINE_DIST
67 #define NAV_HYBRID_NAV_LINE_DIST 50.f
68 #endif
69 
70 #ifndef NAV_HYBRID_NAV_CIRCLE_DIST
71 #define NAV_HYBRID_NAV_CIRCLE_DIST 40.f
72 #endif
73 
74 #ifdef NAV_HYBRID_POS_GAIN
75 float nav_hybrid_pos_gain = NAV_HYBRID_POS_GAIN;
76 #else
77 float nav_hybrid_pos_gain = 1.0;
78 #endif
79 
80 #ifndef GUIDANCE_INDI_HYBRID
81 bool force_forward = 0;
82 #endif
83 
87 static void nav_hybrid_goto(struct EnuCoor_f *wp)
88 {
91  VECT2_COPY(nav.target, *wp);
92 
93  // Calculate position error
94  struct FloatVect2 pos_error;
95  struct EnuCoor_f *pos = stateGetPositionEnu_f();
96  VECT2_DIFF(pos_error, nav.target, *pos);
97 
98  struct FloatVect2 speed_sp;
100 
101  // Bound the setpoint velocity vector
102  float max_h_speed = nav_max_speed;
103  if (!force_forward) {
104  // If not in force_forward, compute speed based on decceleration and nav_goto_max_speed
105  // Calculate distance to waypoint
106  float dist_to_wp = float_vect2_norm(&pos_error);
107  // Calculate max speed when decelerating at MAX capacity a_max
108  // distance travelled d = 1/2 a_max t^2
109  // The time in which it does this is: T = V / a_max
110  // The maximum speed at which to fly to still allow arriving with zero
111  // speed at the waypoint given maximum deceleration is: V = sqrt(2 * a_max * d)
112  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
113  float max_speed_decel = sqrtf(max_speed_decel2);
114  // Bound the setpoint velocity vector
115  max_h_speed = Min(nav_goto_max_speed, max_speed_decel); // use hover max speed
116  }
117  float_vect2_bound_in_2d(&speed_sp, max_h_speed);
118 
122 }
123 
124 static void nav_hybrid_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
125 {
126  struct FloatVect2 wp_diff, pos_diff;
127  VECT2_DIFF(wp_diff, *wp_end, *wp_start);
129 
130  // Calculate magnitude of the desired speed vector based on distance to waypoint
131  float dist_to_target = float_vect2_norm(&pos_diff);
132  float desired_speed;
133  if (force_forward) {
134  desired_speed = nav_max_speed;
135  } else {
136  desired_speed = dist_to_target * nav_hybrid_pos_gain;
137  Bound(desired_speed, 0.0f, nav_max_speed);
138  }
139 
140  // Calculate length of line segment
141  float length_line = Max(float_vect2_norm(&wp_diff), 0.01f);
142  // Normal vector to the line, with length of the line
143  struct FloatVect2 normalv;
144  VECT2_ASSIGN(normalv, -wp_diff.y, wp_diff.x);
145  // Length of normal vector is the same as of the line segment
146  float length_normalv = length_line; // >= 0.01
147  // Distance along the normal vector
148  float dist_to_line = (pos_diff.x * normalv.x + pos_diff.y * normalv.y) / length_normalv;
149  // Normal vector scaled to be the distance to the line
150  struct FloatVect2 v_to_line, v_along_line;
151  v_to_line.x = dist_to_line * normalv.x / length_normalv * nav_hybrid_line_gain;
152  v_to_line.y = dist_to_line * normalv.y / length_normalv * nav_hybrid_line_gain;
153  // The distance that needs to be traveled along the line
154  v_along_line.x = wp_diff.x / length_line * NAV_HYBRID_NAV_LINE_DIST;
155  v_along_line.y = wp_diff.y / length_line * NAV_HYBRID_NAV_LINE_DIST;
156  // Calculate the desired direction to converge to the line
157  struct FloatVect2 direction;
158  VECT2_SMUL(direction, v_along_line, (1.f / (1.f + fabsf(dist_to_line) * 0.05f)));
159  VECT2_ADD(direction, v_to_line);
160  float length_direction = Max(float_vect2_norm(&direction), 0.01f);
161  // Scale to have the desired speed
162  VECT2_SMUL(nav.speed, direction, desired_speed / length_direction);
163  // final target position, should be on the line, for display
165 
166  nav_rotorcraft_base.goto_wp.from = *wp_start;
167  nav_rotorcraft_base.goto_wp.to = *wp_end;
171 }
172 
173 static bool nav_hybrid_approaching(struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time)
174 {
175  float dist_to_point;
176  struct FloatVect2 diff;
177  struct EnuCoor_f *pos = stateGetPositionEnu_f();
178 
179  /* if an approaching_time is given, estimate diff after approching_time secs */
180  if (approaching_time > 0.f) {
181  struct FloatVect2 estimated_pos;
182  struct FloatVect2 estimated_progress;
183  struct EnuCoor_f *speed = stateGetSpeedEnu_f();
184  VECT2_SMUL(estimated_progress, *speed, approaching_time);
185  VECT2_SUM(estimated_pos, *pos, estimated_progress);
186  VECT2_DIFF(diff, *wp, estimated_pos);
187  }
188  /* else use current position */
189  else {
190  VECT2_DIFF(diff, *wp, *pos);
191  }
192  /* compute distance of estimated/current pos to target wp
193  */
194  dist_to_point = float_vect2_norm(&diff);
195 
196  /* return TRUE if we have arrived */
197  if (dist_to_point < ARRIVED_AT_WAYPOINT) {
198  return true;
199  }
200 
201  /* if coming from a valid waypoint */
202  if (from != NULL) {
203  /* return TRUE if normal line at the end of the segment is crossed */
204  struct FloatVect2 from_diff;
205  VECT2_DIFF(from_diff, *wp, *from);
206  return (diff.x * from_diff.x + diff.y * from_diff.y < 0.f);
207  }
208 
209  return false;
210 }
211 
212 static void nav_hybrid_circle(struct EnuCoor_f *wp_center, float radius)
213 {
214  struct FloatVect2 pos_diff;
215  float desired_speed;
216 
217  VECT2_DIFF(pos_diff, *stateGetPositionEnu_f(), *wp_center);
218  // direction of rotation
219  float sign_radius = radius > 0.f ? 1.f : -1.f;
220  // absolute radius
221  float abs_radius = fabsf(radius);
222 
223  if (abs_radius > 0.1f) {
224  // store last qdr
225  float last_qdr = nav_rotorcraft_base.circle.qdr;
226  // compute qdr
228  // increment circle radians
229  float trigo_diff = nav_rotorcraft_base.circle.qdr - last_qdr;
230  NormRadAngle(trigo_diff);
231  nav_rotorcraft_base.circle.radians += trigo_diff;
232  // progress angle
233  float progress_angle = NAV_HYBRID_NAV_CIRCLE_DIST / abs_radius;
234  Bound(progress_angle, M_PI / 16.f, M_PI / 4.f);
235  float alpha = nav_rotorcraft_base.circle.qdr - sign_radius * progress_angle;
236  // final target position, should be on the circle, for display
237  nav.target.x = wp_center->x + cosf(alpha) * abs_radius;
238  nav.target.y = wp_center->y + sinf(alpha) * abs_radius;
239  }
240  else {
241  // radius is too small, direct to center
242  VECT2_COPY(nav.target, *wp_center);
243  }
244  // compute desired speed
245  if (force_forward) {
246  desired_speed = nav_max_speed;
247  } else {
248  float radius_diff = fabsf(float_vect2_norm(&pos_diff) - abs_radius);
249  if (radius_diff > NAV_HYBRID_NAV_CIRCLE_DIST) {
250  // far from circle, speed proportional to diff
251  desired_speed = radius_diff * nav_hybrid_pos_gain;
252  } else {
253  // close to circle, speed function of radius for a feasible turn
254  // 0.8 * MAX_BANK gives some margins for the turns
255  desired_speed = sqrtf(PPRZ_ISA_GRAVITY * abs_radius * tanf(0.8f * NAV_HYBRID_MAX_BANK));
256  }
257  Bound(desired_speed, 0.0f, nav_max_speed);
258  }
259  // compute speed vector from target position
260  struct FloatVect2 speed_unit;
261  VECT2_DIFF(speed_unit, nav.target, *stateGetPositionEnu_f());
262  float_vect2_normalize(&speed_unit);
263  VECT2_SMUL(nav.speed, speed_unit, desired_speed);
264 
265  nav_rotorcraft_base.circle.center = *wp_center;
269 }
270 
278 {
282 
283  // register nav functions
286 }
287 
288 
#define Min(x, y)
Definition: esc_dshot.c:101
void float_vect2_bound_in_2d(struct FloatVect2 *vect2, float bound)
static float float_vect2_norm(struct FloatVect2 *v)
static void float_vect2_normalize(struct FloatVect2 *v)
normalize 2D vector in place
#define VECT2_ADD(_a, _b)
Definition: pprz_algebra.h:74
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:98
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
#define VECT2_SUM(_c, _a, _b)
Definition: pprz_algebra.h:86
#define VECT2_ASSIGN(_a, _x, _y)
Definition: pprz_algebra.h:62
#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:719
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
struct FloatVect3 speed_sp
Definition: guidance_indi.c:77
float leg_progress
progress over leg
Definition: nav_base.h:39
struct EnuCoor_f center
center WP position
Definition: nav_base.h:46
struct NavGoto_t goto_wp
Definition: nav_base.h:63
float leg_length
leg length
Definition: nav_base.h:40
struct EnuCoor_f to
end WP position
Definition: nav_base.h:37
float dist2_to_wp
squared distance to next waypoint
Definition: nav_base.h:38
struct NavCircle_t circle
Definition: nav_base.h:64
struct EnuCoor_f from
start WP position
Definition: nav_base.h:36
float radians
incremental angular distance
Definition: nav_base.h:49
float radius
radius in meters
Definition: nav_base.h:47
float qdr
qdr in radians
Definition: nav_base.h:48
struct NavBase_t nav_rotorcraft_base
Basic Nav struct.
float nav_max_deceleration_sp
bool force_forward
forward flight for hybrid nav
static void nav_hybrid_circle(struct EnuCoor_f *wp_center, float radius)
static void nav_hybrid_goto(struct EnuCoor_f *wp)
Implement basic nav function for the hybrid case.
#define NAV_HYBRID_MAX_BANK
#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
#define NAV_HYBRID_GOTO_MAX_SPEED
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)
float nav_goto_max_speed
void nav_rotorcraft_hybrid_init(void)
Init and register nav functions.
#define NAV_HYBRID_MAX_DECELERATION
#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:397
void nav_register_goto_wp(navigation_goto nav_goto, navigation_route nav_route, navigation_approaching nav_approaching)
Registering functions.
Definition: navigation.c:390
float get_dist2_to_point(struct EnuCoor_f *p)
Returns squared horizontal distance to given point.
Definition: navigation.c:306
Rotorcraft navigation functions.
struct EnuCoor_f speed
speed setpoint (in m/s)
Definition: navigation.h:128
#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
struct EnuCoor_f target
final target position (in meters)
Definition: navigation.h:126
#define ARRIVED_AT_WAYPOINT
minimum horizontal distance to waypoint to mark as arrived
Definition: navigation.h:55
float alpha
Definition: textons.c:133