Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
nav_rotorcraft_base.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2022 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 #include "generated/flight_plan.h"
30 
32 
35 static void nav_stage_init(void)
36 {
39 }
40 
41 static void nav_goto(struct EnuCoor_f *wp)
42 {
45  VECT2_COPY(nav.target, *wp);
48 }
49 
50 static void nav_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
51 {
52  struct FloatVect2 wp_diff, pos_diff;
53  VECT2_DIFF(wp_diff, *wp_end, *wp_start);
55  // leg length
56  float leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y), 0.1f);
57  nav_rotorcraft_base.goto_wp.leg_length = sqrtf(leg_length2);
58  // leg progress
61  // next pos on leg
62  struct FloatVect2 progress_pos;
64  VECT2_SUM(nav.target, *wp_start, progress_pos);
65 
66  nav_rotorcraft_base.goto_wp.from = *wp_start;
67  nav_rotorcraft_base.goto_wp.to = *wp_end;
71 }
72 
73 static bool nav_approaching(struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time)
74 {
75  float dist_to_point;
76  struct FloatVect2 diff;
77  struct EnuCoor_f *pos = stateGetPositionEnu_f();
78 
79  /* if an approaching_time is given, estimate diff after approching_time secs */
80  if (approaching_time > 0.f) {
81  struct FloatVect2 estimated_pos;
82  struct FloatVect2 estimated_progress;
83  struct EnuCoor_f *speed = stateGetSpeedEnu_f();
84  VECT2_SMUL(estimated_progress, *speed, approaching_time);
85  VECT2_SUM(estimated_pos, *pos, estimated_progress);
86  VECT2_DIFF(diff, *wp, estimated_pos);
87  }
88  /* else use current position */
89  else {
90  VECT2_DIFF(diff, *wp, *pos);
91  }
92  /* compute distance of estimated/current pos to target wp
93  */
94  dist_to_point = float_vect2_norm(&diff);
95 
96  /* return TRUE if we have arrived */
97  if (dist_to_point < ARRIVED_AT_WAYPOINT) {
98  return true;
99  }
100 
101  /* if coming from a valid waypoint */
102  if (from != NULL) {
103  /* return TRUE if normal line at the end of the segment is crossed
104  * if 'from' and 'to' WP are the same, diff is zero and function should
105  * return false (only distance to waypoint will be considered) */
106  struct FloatVect2 from_diff;
107  VECT2_DIFF(from_diff, *wp, *from);
108  return (diff.x * from_diff.x + diff.y * from_diff.y < -0.01f);
109  }
110 
111  return false;
112 }
113 
114 static void nav_circle(struct EnuCoor_f *wp_center, float radius)
115 {
116  struct FloatVect2 pos_diff;
117 
118  if (fabsf(radius) < 0.001f) {
119  VECT2_COPY(nav.target, *wp_center);
120  } else {
121  VECT2_DIFF(pos_diff, *stateGetPositionEnu_f(), *wp_center);
122  // store last qdr
123  float last_qdr = nav_rotorcraft_base.circle.qdr;
124  // compute qdr
126  // increment circle radians
127  float trigo_diff = nav_rotorcraft_base.circle.qdr - last_qdr;
128  NormRadAngle(trigo_diff);
129  nav_rotorcraft_base.circle.radians += trigo_diff;
130  }
131 
132  // direction of rotation
133  float sign_radius = radius > 0.f ? 1.f : -1.f;
134  // absolute radius
135  float abs_radius = fabsf(radius);
136  if (abs_radius > 0.1f) {
137  // carrot_angle
138  float carrot_angle = NAV_CARROT_DIST / abs_radius;
139  carrot_angle = Min(carrot_angle, M_PI / 4);
140  carrot_angle = Max(carrot_angle, M_PI / 16);
141  carrot_angle = nav_rotorcraft_base.circle.qdr - sign_radius * carrot_angle;
142  // compute setpoint
143  VECT2_ASSIGN(pos_diff, abs_radius * cosf(carrot_angle), abs_radius * sinf(carrot_angle));
144  VECT2_SUM(nav.target, *wp_center, pos_diff);
145  }
146  else {
147  // radius is too small, direct to center
148  VECT2_COPY(nav.target, *wp_center);
149  }
150 
151  nav_rotorcraft_base.circle.center = *wp_center;
155 }
156 
163 #ifndef LINE_START_FUNCTION
164 #define LINE_START_FUNCTION {}
165 #endif
166 #ifndef LINE_STOP_FUNCTION
167 #define LINE_STOP_FUNCTION {}
168 #endif
169 
170 static void _nav_oval_init(void)
171 {
174 }
175 
176 static void nav_oval(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
177 {
178  float alt = wp1->z;
179  wp2->z = alt;
180 
181  /* Unit vector from p1 to p2 */
182  struct FloatVect2 dir;
183  VECT2_DIFF(dir, *wp1, *wp2);
185 
186  /* The half circle centers and the other leg */
187  struct EnuCoor_f p1_center = {
188  wp1->x + radius * dir.y,
189  wp1->y - radius * dir.x,
190  alt
191  };
192  struct EnuCoor_f p1_out = {
193  wp1->x + 2.f * radius * dir.y,
194  wp1->y - 2.f * radius * dir.x,
195  alt
196  };
197  struct EnuCoor_f p2_in = {
198  wp2->x + 2.f * radius * dir.y,
199  wp2->y - 2.f * radius * dir.x,
200  alt
201  };
202  struct EnuCoor_f p2_center = {
203  wp2->x + radius * dir.y,
204  wp2->y - radius * dir.x,
205  alt
206  };
207 
208  float qdr_out_2 = M_PI - atan2f(dir.y, dir.x);
209  float qdr_out_1 = qdr_out_2 + M_PI;
210  if (radius > 0.f) {
211  qdr_out_2 += M_PI;
212  qdr_out_1 += M_PI;
213  }
214  float qdr_anticipation = (radius > 0.f ? 15.f : -15.f);
215 
216  switch (nav_rotorcraft_base.oval.status) {
217  case OC1 :
218  nav.nav_circle(&p1_center, radius);
219  if (NavQdrCloseTo(DegOfRad(qdr_out_1) - qdr_anticipation)) {
221  InitStage();
223  }
224  return;
225 
226  case OR12:
227  nav.nav_route(&p1_out, &p2_in);
228  if (nav.nav_approaching(&p2_in, &p1_out, CARROT)) {
231  InitStage();
233  }
234  return;
235 
236  case OC2 :
237  nav.nav_circle(&p2_center, radius);
238  if (NavQdrCloseTo(DegOfRad(qdr_out_2) - qdr_anticipation)) {
240  InitStage();
242  }
243  return;
244 
245  case OR21:
246  nav.nav_route(wp2, wp1);
247  if (nav.nav_approaching(wp1, wp2, CARROT)) {
249  InitStage();
251  }
252  return;
253 
254  default: /* Should not occur !!! Doing nothing */
255  return;
256  }
257 }
258 
259 #if PERIODIC_TELEMETRY
261 
262 static void send_segment(struct transport_tx *trans, struct link_device *dev)
263 {
264  pprz_msg_send_SEGMENT(trans, dev, AC_ID,
269 }
270 
271 static void send_circle(struct transport_tx *trans, struct link_device *dev)
272 {
273  pprz_msg_send_CIRCLE(trans, dev, AC_ID,
277 }
278 
279 static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
280 {
281  float dist_home = sqrtf(nav.dist2_to_home);
282  float dist_wp = sqrtf(nav_rotorcraft_base.goto_wp.dist2_to_wp);
283  pprz_msg_send_ROTORCRAFT_NAV_STATUS(trans, dev, AC_ID,
285  &dist_home, &dist_wp,
286  &nav_block, &nav_stage,
289  send_segment(trans, dev);
291  send_circle(trans, dev);
292  }
293 }
294 #endif
295 
299 {
303 
304  // register nav functions
309 
310 #if PERIODIC_TELEMETRY
311  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_NAV_STATUS, send_nav_status);
312 #endif
313 
314 }
315 
316 
uint8_t nav_stage
uint8_t nav_block
uint16_t stage_time
In s.
uint16_t block_time
#define InitStage()
#define Min(x, y)
Definition: esc_dshot.c:109
static float float_vect2_norm(struct FloatVect2 *v)
static void float_vect2_normalize(struct FloatVect2 *v)
normalize 2D vector in place
#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
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
@ OC2
Definition: nav.h:60
@ OC1
Definition: nav.h:60
@ OR12
Definition: nav.h:60
@ OR21
Definition: nav.h:60
#define NavQdrCloseTo(x)
True if x (in degrees) is close to the current QDR (less than 10 degrees)
Definition: nav.h:161
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
uint8_t count
number of laps
Definition: nav_base.h:57
float leg_length
leg length
Definition: nav_base.h:40
struct NavOval_t oval
Definition: nav_base.h:65
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
enum oval_status status
oval status
Definition: nav_base.h:56
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
Basic Nav struct.
Definition: nav_base.h:62
static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
#define LINE_STOP_FUNCTION
struct NavBase_t nav_rotorcraft_base
Basic Nav struct.
static void nav_goto(struct EnuCoor_f *wp)
static void send_circle(struct transport_tx *trans, struct link_device *dev)
#define LINE_START_FUNCTION
Navigation along a figure O.
static void _nav_oval_init(void)
static void nav_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
static void nav_circle(struct EnuCoor_f *wp_center, float radius)
void nav_rotorcraft_init(void)
Init and register nav functions.
static void send_segment(struct transport_tx *trans, struct link_device *dev)
static bool nav_approaching(struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time)
static void nav_stage_init(void)
Implement basic nav function.
static void nav_oval(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
struct FloatVect2 pos_diff
float y
in meters
float x
in meters
float z
in meters
vector in East North Up coordinates Units: meters
void nav_register_oval(navigation_oval_init _nav_oval_init, navigation_oval nav_oval)
Definition: navigation.c:404
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)
Registering functions.
Definition: navigation.c:392
float get_dist2_to_point(struct EnuCoor_f *p)
Returns squared horizontal distance to given point.
Definition: navigation.c:308
void nav_register_stage_init(navigation_stage_init nav_stage_init)
Register functions.
Definition: navigation.c:387
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
float dist2_to_home
squared distance to home waypoint
Definition: navigation.h:143
#define NAV_HORIZONTAL_MODE_ROUTE
Definition: navigation.h:86
navigation_approaching nav_approaching
Definition: navigation.h:153
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
Definition: navigation.h:42
navigation_circle nav_circle
Definition: navigation.h:154
#define CARROT
default approaching_time for a wp
Definition: navigation.h:70
#define NAV_CARROT_DIST
Carrot distance during navigation.
Definition: navigation.h:65
navigation_route nav_route
Definition: navigation.h:152
#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
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
static const float dir[]
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66