Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
26
29#include "generated/flight_plan.h"
30
32
35static void nav_stage_init(void)
36{
37 nav_rotorcraft_base.circle.radians = 0.f;
38 nav_rotorcraft_base.goto_wp.leg_progress = 0.f;
39}
40
41static void nav_goto(struct EnuCoor_f *wp)
42{
43 nav_rotorcraft_base.goto_wp.to = *wp;
44 nav_rotorcraft_base.goto_wp.dist2_to_wp = get_dist2_to_point(wp);
45 VECT2_COPY(nav.target, *wp);
46 nav.horizontal_mode = NAV_HORIZONTAL_MODE_WAYPOINT;
47 nav.setpoint_mode = NAV_SETPOINT_MODE_POS;
48}
49
50static 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
59 nav_rotorcraft_base.goto_wp.leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / nav_rotorcraft_base.goto_wp.leg_length;
60 nav_rotorcraft_base.goto_wp.leg_progress += Max(NAV_CARROT_DIST, 0.f);
61 // next pos on leg
62 struct FloatVect2 progress_pos;
63 VECT2_SMUL(progress_pos, wp_diff, nav_rotorcraft_base.goto_wp.leg_progress / nav_rotorcraft_base.goto_wp.leg_length);
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;
68 nav_rotorcraft_base.goto_wp.dist2_to_wp = get_dist2_to_point(wp_end);
69 nav.horizontal_mode = NAV_HORIZONTAL_MODE_ROUTE;
70 nav.setpoint_mode = NAV_SETPOINT_MODE_POS;
71}
72
73static 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
114static 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 {
122 // store last qdr
123 float last_qdr = nav_rotorcraft_base.circle.qdr;
124 // compute qdr
125 nav_rotorcraft_base.circle.qdr = atan2f(pos_diff.y, pos_diff.x);
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;
152 nav_rotorcraft_base.circle.radius = radius;
153 nav.horizontal_mode = NAV_HORIZONTAL_MODE_CIRCLE;
154 nav.setpoint_mode = NAV_SETPOINT_MODE_POS;
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
170static void _nav_oval_init(void)
171{
172 nav_rotorcraft_base.oval.status = OC2;
173 nav_rotorcraft_base.oval.count = 0;
174}
175
176static 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)) {
220 nav_rotorcraft_base.oval.status = OR12;
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)) {
229 nav_rotorcraft_base.oval.status = OC2;
230 nav_rotorcraft_base.oval.count++;
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)) {
239 nav_rotorcraft_base.oval.status = OR21;
240 InitStage();
242 }
243 return;
244
245 case OR21:
246 nav.nav_route(wp2, wp1);
247 if (nav.nav_approaching(wp1, wp2, CARROT)) {
248 nav_rotorcraft_base.oval.status = OC1;
249 InitStage();
251 }
252 return;
253
254 default: /* Should not occur !!! Doing nothing */
255 return;
256 }
257}
258
259#if PERIODIC_TELEMETRY
261
262#if ROTORCRAFT_BASE_SEND_TRAJECTORY
263static void send_segment(struct transport_tx *trans, struct link_device *dev)
264{
265 pprz_msg_send_SEGMENT(trans, dev, AC_ID,
266 &nav_rotorcraft_base.goto_wp.from.x,
267 &nav_rotorcraft_base.goto_wp.from.y,
268 &nav_rotorcraft_base.goto_wp.to.x,
269 &nav_rotorcraft_base.goto_wp.to.y);
270}
271
272static void send_circle(struct transport_tx *trans, struct link_device *dev)
273{
274 pprz_msg_send_CIRCLE(trans, dev, AC_ID,
275 &nav_rotorcraft_base.circle.center.x,
276 &nav_rotorcraft_base.circle.center.y,
277 &nav_rotorcraft_base.circle.radius);
278}
279# endif // ROTORCRAFT_BASE_SEND_TRAJECTORY
280
281static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
282{
283 float dist_home = sqrtf(nav.dist2_to_home);
284 float dist_wp = sqrtf(nav_rotorcraft_base.goto_wp.dist2_to_wp);
285 pprz_msg_send_ROTORCRAFT_NAV_STATUS(trans, dev, AC_ID,
287 &dist_home, &dist_wp,
289 &nav.horizontal_mode);
290#if ROTORCRAFT_BASE_SEND_TRAJECTORY
291 if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_ROUTE) {
292 send_segment(trans, dev);
293 } else if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_CIRCLE) {
294 send_circle(trans, dev);
295 }
296#endif // ROTORCRAFT_BASE_SEND_TRAJECTORY
297}
298#endif
299
303{
305 nav_rotorcraft_base.goto_wp.leg_progress = 0.f;
306 nav_rotorcraft_base.goto_wp.leg_length = 1.f;
307
308 // register nav functions
313
314#if PERIODIC_TELEMETRY
315 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_NAV_STATUS, send_nav_status);
316#endif
317
318}
319
320
uint8_t nav_stage
uint8_t nav_block
uint16_t stage_time
In s.
uint16_t block_time
#define InitStage()
static struct uart_periph * dev
#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)
#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)
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
#define LINE_STOP_FUNCTION
Definition nav.c:279
static void send_circle(struct transport_tx *trans, struct link_device *dev)
Definition nav.c:504
#define LINE_START_FUNCTION
Definition nav.c:276
static void send_segment(struct transport_tx *trans, struct link_device *dev)
Definition nav.c:512
void nav_oval(uint8_t p1, uint8_t p2, float radius)
Definition nav.c:762
@ 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
Basic Nav struct.
Definition nav_base.h:62
static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
struct NavBase_t nav_rotorcraft_base
Basic Nav struct.
static void nav_goto(struct EnuCoor_f *wp)
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 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)
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
#define NAV_HORIZONTAL_MODE_ROUTE
Definition navigation.h:86
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
Definition navigation.h:42
#define CARROT
default approaching_time for a wp
Definition navigation.h:70
#define NAV_CARROT_DIST
Carrot distance during navigation.
Definition navigation.h:65
#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
static const float dir[]
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_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