Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
nav_rover_base.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2018 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
28
30
33
34static void nav_stage_init(void)
35{
36 nav_rover_base.circle.radians = 0.f;
37 nav_rover_base.goto_wp.leg_progress = 0.f;
38}
39
40static void nav_goto(struct EnuCoor_f *wp)
41{
42 nav_rover_base.goto_wp.to = *wp;
43 nav_rover_base.goto_wp.dist2_to_wp = get_dist2_to_point(wp);
44 VECT3_COPY(nav.target, *wp);
46}
47
48static void nav_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
49{
50 struct FloatVect2 wp_diff, pos_diff;
51 VECT2_DIFF(wp_diff, *wp_end, *wp_start);
53 // leg length
54 float leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y), 0.1f);
55 nav_rover_base.goto_wp.leg_length = sqrtf(leg_length2);
56 // leg progress
57 nav_rover_base.goto_wp.leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / nav_rover_base.goto_wp.leg_length;
58 nav_rover_base.goto_wp.leg_progress += Max(CARROT_DIST, 0.f);
59 // next pos on leg
60 struct FloatVect2 progress_pos;
61 VECT2_SMUL(progress_pos, wp_diff, nav_rover_base.goto_wp.leg_progress / nav_rover_base.goto_wp.leg_length);
62 VECT2_SUM(nav.target, *wp_start, progress_pos);
63
64 nav_rover_base.goto_wp.from = *wp_start;
65 nav_rover_base.goto_wp.to = *wp_end;
66 nav_rover_base.goto_wp.dist2_to_wp = get_dist2_to_point(wp_end);
67 nav.mode = NAV_MODE_ROUTE;
68}
69
70static bool nav_approaching(struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time)
71{
72 float dist_to_point;
73 struct FloatVect2 diff;
74 struct EnuCoor_f *pos = stateGetPositionEnu_f();
75
76 /* if an approaching_time is given, estimate diff after approching_time secs */
77 if (approaching_time > 0.f) {
78 struct FloatVect2 estimated_pos;
79 struct FloatVect2 estimated_progress;
80 struct EnuCoor_f *speed = stateGetSpeedEnu_f();
81 VECT2_SMUL(estimated_progress, *speed, approaching_time);
82 VECT2_SUM(estimated_pos, *pos, estimated_progress);
83 VECT2_DIFF(diff, *wp, estimated_pos);
84 }
85 /* else use current position */
86 else {
87 VECT2_DIFF(diff, *wp, *pos);
88 }
89 /* compute distance of estimated/current pos to target wp
90 */
91 //nav.dist_to_point = float_vect2_norm(&diff); FIXME
92 dist_to_point = float_vect2_norm(&diff);
93
94 /* return TRUE if we have arrived */
95 if (dist_to_point < ARRIVED_AT_WAYPOINT) {
96 return true;
97 }
98
99 /* if coming from a valid waypoint */
100 if (from != NULL) {
101 /* return TRUE if normal line at the end of the segment is crossed */
102 struct FloatVect2 from_diff;
103 VECT2_DIFF(from_diff, *wp, *from);
104 return (diff.x * from_diff.x + diff.y * from_diff.y < 0.f);
105 }
106
107 return false;
108}
109
110static void nav_circle(struct EnuCoor_f *wp_center, float radius)
111{
112 struct FloatVect2 pos_diff;
114 // store last qdr
115 float last_qdr = nav_rover_base.circle.qdr;
116 // compute qdr
117 nav_rover_base.circle.qdr = atan2f(pos_diff.y, pos_diff.x);
118 // increment circle radians
119 float trigo_diff = nav_rover_base.circle.qdr - last_qdr;
120 NormRadAngle(trigo_diff);
121 nav_rover_base.circle.radians += trigo_diff;
122
123 // direction of rotation
124 float sign_radius = radius > 0.f ? 1.f : -1.f;
125 // absolute radius
126 float abs_radius = fabsf(radius);
127 if (abs_radius > 0.1f) {
128 // carrot_angle
129 float carrot_angle = CARROT_DIST / abs_radius;
130 carrot_angle = Min(carrot_angle, M_PI / 4);
131 carrot_angle = Max(carrot_angle, M_PI / 16);
132 carrot_angle = nav_rover_base.circle.qdr - sign_radius * carrot_angle;
133 // compute setpoint
134 VECT2_ASSIGN(pos_diff, abs_radius * cosf(carrot_angle), abs_radius * sinf(carrot_angle));
135 VECT2_SUM(nav.target, *wp_center, pos_diff);
136 }
137 else {
138 // radius is too small, direct to center
139 VECT2_COPY(nav.target, *wp_center);
140 }
141
142 nav_rover_base.circle.center = *wp_center;
143 nav_rover_base.circle.radius = radius;
144 nav.mode = NAV_MODE_CIRCLE;
145}
146
153#ifndef LINE_START_FUNCTION
154#define LINE_START_FUNCTION {}
155#endif
156#ifndef LINE_STOP_FUNCTION
157#define LINE_STOP_FUNCTION {}
158#endif
159
160static void _nav_oval_init(void)
161{
162 nav_rover_base.oval.status = OC2;
163 nav_rover_base.oval.count = 0;
164}
165
166static void nav_oval(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
167{
168 float alt = wp1->z;
169 wp2->z = alt;
170
171 struct FloatVect2 dir;
172 VECT2_DIFF(dir, *wp1, *wp2);
174
175 /* The half circle centers and the other leg */
176 struct EnuCoor_f p1_center = {
177 wp1->x + radius * dir.y,
178 wp1->y - radius * dir.x,
179 alt
180 };
181 struct EnuCoor_f p1_out = {
182 wp1->x + 2.f * radius * dir.y,
183 wp1->y - 2.f * radius * dir.x,
184 alt
185 };
186 struct EnuCoor_f p2_in = {
187 wp2->x + 2.f * radius * dir.y,
188 wp2->y - 2.f * radius * dir.x,
189 alt
190 };
191 struct EnuCoor_f p2_center = {
192 wp2->x + radius * dir.y,
193 wp2->y - radius * dir.x,
194 alt
195 };
196
197 float qdr_out_2 = M_PI - atan2f(dir.y, dir.x);
198 float qdr_out_1 = qdr_out_2 + M_PI;
199 if (radius > 0.f) {
200 qdr_out_2 += M_PI;
201 qdr_out_1 += M_PI;
202 }
203 float qdr_anticipation = (radius > 0.f ? 15.f : -15.f);
204
205 switch (nav_rover_base.oval.status) {
206 case OC1 :
207 nav.nav_circle(&p1_center, radius);
208 if (NavQdrCloseTo(DegOfRad(qdr_out_1) - qdr_anticipation)) {
209 nav_rover_base.oval.status = OR12;
210 InitStage();
212 }
213 return;
214
215 case OR12:
216 nav.nav_route(&p1_out, &p2_in);
217 if (nav.nav_approaching(&p2_in, &p1_out, CARROT)) {
218 nav_rover_base.oval.status = OC2;
219 nav_rover_base.oval.count++;
220 InitStage();
222 }
223 return;
224
225 case OC2 :
226 nav.nav_circle(&p2_center, radius);
227 if (NavQdrCloseTo(DegOfRad(qdr_out_2) - qdr_anticipation)) {
228 nav_rover_base.oval.status = OR21;
229 InitStage();
231 }
232 return;
233
234 case OR21:
235 nav.nav_route(wp2, wp1);
236 if (nav.nav_approaching(wp1, wp2, CARROT)) {
237 nav_rover_base.oval.status = OC1;
238 InitStage();
240 }
241 return;
242
243 default: /* Should not occur !!! Doing nothing */
244 return;
245 }
246}
247
248#if PERIODIC_TELEMETRY
250
251#if ROVER_BASE_SEND_TRAJECTORY
252static void send_segment(struct transport_tx *trans, struct link_device *dev)
253{
254 pprz_msg_send_SEGMENT(trans, dev, AC_ID,
255 &nav_rover_base.goto_wp.from.x,
256 &nav_rover_base.goto_wp.from.y,
257 &nav_rover_base.goto_wp.to.x,
258 &nav_rover_base.goto_wp.to.y);
259}
260
261static void send_circle(struct transport_tx *trans, struct link_device *dev)
262{
263 pprz_msg_send_CIRCLE(trans, dev, AC_ID,
264 &nav_rover_base.circle.center.x,
265 &nav_rover_base.circle.center.y,
266 &nav_rover_base.circle.radius);
267}
268#endif // ROVER_BASE_SEND_TRAJECTORY
269
270static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
271{
272 float dist_home = sqrtf(nav.dist2_to_home);
273 float dist_wp = sqrtf(nav_rover_base.goto_wp.dist2_to_wp);
274 pprz_msg_send_ROTORCRAFT_NAV_STATUS(trans, dev, AC_ID,
276 &dist_home, &dist_wp,
278 &nav.mode);
279#if ROVER_BASE_SEND_TRAJECTORY
280 if (nav.mode == NAV_MODE_ROUTE) {
281 send_segment(trans, dev);
282 } else if (nav.mode == NAV_MODE_CIRCLE) {
283 send_circle(trans, dev);
284 }
285#endif // ROVER_BASE_SEND_TRAJECTORY
286}
287#endif
288
292{
294 nav_rover_base.goto_wp.leg_progress = 0.f;
295 nav_rover_base.goto_wp.leg_length = 1.f;
296
297 // register nav functions
302
303#if PERIODIC_TELEMETRY
304 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_NAV_STATUS, send_nav_status);
305#endif
306
307}
308
309
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 VECT3_COPY(_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
static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
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)
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 send_nav_status(struct transport_tx *trans, struct link_device *dev)
static void nav_goto(struct EnuCoor_f *wp)
void nav_rover_init(void)
Init and register nav functions.
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)
struct RoverNavBase nav_rover_base
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)
Basic Nav struct.
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
#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 ARRIVED_AT_WAYPOINT
minimum horizontal distance to waypoint to mark as arrived
Definition navigation.h:55
#define NAV_MODE_CIRCLE
Definition navigation.h:74
#define CARROT_DIST
Definition navigation.h:43
#define NAV_MODE_ROUTE
Definition navigation.h:73
#define NAV_MODE_WAYPOINT
Nav modes.
Definition navigation.h:72
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