Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
mission_rotorcraft_nav.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2014 Kadir Cimenci
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, write to
18 * the Free Software Foundation, 59 Temple Place - Suite 330,
19 * Boston, MA 02111-1307, USA.
20 *
21 */
22
29
30#include <stdio.h>
32#include "autopilot.h"
34#include "generated/flight_plan.h"
35
36
37//Buffer zone in [m] before MAX_DIST_FROM_HOME
38#define BUFFER_ZONE_DIST 10
39
42{
43 // return FALSE if there is no valid local coordinate system
44 if (!state.ned_initialized_i) {
45 return false;
46 }
47
48 // change geoid alt to ellipsoid alt
50
51 //Compute ENU components from LLA with respect to ltp origin
52 struct EnuCoor_i tmp_enu_point_i;
53 enu_of_lla_point_i(&tmp_enu_point_i, stateGetNedOrigin_i(), lla);
54 struct EnuCoor_f tmp_enu_point_f;
55 // result of enu_of_lla_point_i is in cm, convert to float in m
56 VECT3_SMUL(tmp_enu_point_f, tmp_enu_point_i, 0.01);
57
58 //Bound the new waypoint with max distance from home
59 struct FloatVect2 home;
60 home.x = waypoint_get_x(WP_HOME);
61 home.y = waypoint_get_y(WP_HOME);
62 struct FloatVect2 vect_from_home;
63 VECT2_DIFF(vect_from_home, tmp_enu_point_f, home);
64 //Saturate the mission wp not to overflow max_dist_from_home
65 //including a buffer zone before limits
66 float dist_to_home = float_vect2_norm(&vect_from_home);
67 dist_to_home += BUFFER_ZONE_DIST;
68 if (dist_to_home > MAX_DIST_FROM_HOME) {
69 VECT2_SMUL(vect_from_home, vect_from_home, (MAX_DIST_FROM_HOME / dist_to_home));
70 }
71 // set new point
72 VECT2_SUM(*point, home, vect_from_home);
73 point->z = tmp_enu_point_f.z;
74
75 return true;
76}
77
78// navigation time step
79static const float dt_navigation = 1.0f / ((float)NAVIGATION_FREQUENCY);
80
81// last_mission_wp, last target wp from mission elements, not used actively and kept for future implementations
82struct EnuCoor_f last_mission_wp = { 0.f, 0.f, 0.f };
83
86static inline bool mission_nav_wp(struct _mission_element *el)
87{
88 struct EnuCoor_f *target_wp = &(el->element.mission_wp.wp);
89
90 //Check proximity and wait for 'duration' seconds in proximity circle if desired
91 if (nav.nav_approaching(target_wp, NULL, CARROT)
92#ifdef MISSION_ALT_PROXIMITY
93 && fabsf(stateGetPositionEnu_f()->z - target_wp->z) <= MISSION_ALT_PROXIMITY
94#endif
95 ) {
96 last_mission_wp = *target_wp;
97
98 if (el->duration > 0.f) {
99 if (nav_check_wp_time(target_wp, el->duration)) { return false; }
100 } else { return false; }
101
102 }
103 //Go to Mission Waypoint
104 nav.nav_goto(target_wp);
105 NavVerticalAutoThrottleMode(RadOfDeg(0.f));
106 NavVerticalAltitudeMode(target_wp->z, 0.f);
107
108 return true;
109}
110
113static inline bool mission_nav_circle(struct _mission_element *el)
114{
115 struct EnuCoor_f *center_wp = &(el->element.mission_circle.center);
116 float radius = el->element.mission_circle.radius;
117
118 //Draw the desired circle for a 'duration' time
119 nav.nav_circle(center_wp, radius);
120 NavVerticalAutoThrottleMode(RadOfDeg(0.f));
121 NavVerticalAltitudeMode(center_wp->z, 0.f);
122
123 if (el->duration > 0.f && mission.element_time >= el->duration) {
124 return false;
125 }
126
127 return true;
128}
129
132static inline bool mission_nav_segment(struct _mission_element *el)
133{
134 struct EnuCoor_f *from_wp = &(el->element.mission_segment.from);
135 struct EnuCoor_f *to_wp = &(el->element.mission_segment.to);
136
137 //Check proximity and wait for 'duration' seconds in proximity circle if desired
138 if (nav.nav_approaching(to_wp, from_wp, CARROT)
139#ifdef MISSION_ALT_PROXIMITY
140 && fabsf(stateGetPositionEnu_f()->z - to_wp->z) <= MISSION_ALT_PROXIMITY
141#endif
142 ) {
143 last_mission_wp = *to_wp;
144
145 if (el->duration > 0.f) {
146 if (nav_check_wp_time(to_wp, el->duration)) { return false; }
147 } else { return false; }
148 }
149
150 //Route Between from-to
151 nav.nav_route(from_wp, to_wp);
152 NavVerticalAutoThrottleMode(RadOfDeg(0.f));
153 NavVerticalAltitudeMode(to_wp->z, 0.f);
154
155 return true;
156}
157
158
159#ifndef MISSION_PATH_SKIP_GOTO
160#define MISSION_PATH_SKIP_GOTO FALSE
161#endif
162
165static inline bool mission_nav_path(struct _mission_element *el)
166{
167 if (el->element.mission_path.nb == 0) {
168 return false; // nothing to do
169 }
170
171 if (el->element.mission_path.path_idx == 0) { //first wp of path
172 el->element.mission_wp.wp = el->element.mission_path.path[0];
173 if (MISSION_PATH_SKIP_GOTO || !mission_nav_wp(el)) { el->element.mission_path.path_idx++; }
174 }
175
176 else if (el->element.mission_path.path_idx < el->element.mission_path.nb) { //standart wp of path
177
178 struct EnuCoor_f *from_wp = &(el->element.mission_path.path[(el->element.mission_path.path_idx) - 1]);
179 struct EnuCoor_f *to_wp = &(el->element.mission_path.path[el->element.mission_path.path_idx]);
180
181 //Check proximity and wait for t seconds in proximity circle if desired
182 if (nav.nav_approaching(to_wp, from_wp, CARROT)
183#ifdef MISSION_ALT_PROXIMITY
184 && fabsf(stateGetPositionEnu_f()->z - from_wp->z) <= MISSION_ALT_PROXIMITY
185#endif
186 ) {
187 last_mission_wp = *to_wp;
188
189 if (el->duration > 0.f) {
190 if (nav_check_wp_time(to_wp, el->duration)) {
191 el->element.mission_path.path_idx++;
192 }
193 } else { el->element.mission_path.path_idx++; }
194 }
195 //Route Between from-to
196 nav.nav_route(from_wp, to_wp);
197 NavVerticalAutoThrottleMode(RadOfDeg(0.f));
198 NavVerticalAltitudeMode(from_wp->z, 0.f);
199 } else { return false; } //end of path
200
201 return true;
202}
203
206static inline bool mission_nav_custom(struct _mission_custom *custom, bool init)
207{
208 if (init) {
209 return custom->reg->cb(custom->nb, custom->params, MissionInit);
210 } else {
211 return custom->reg->cb(custom->nb, custom->params, MissionRun);
212 }
213}
214
218#ifndef MISSION_WAIT_TIMEOUT
219#define MISSION_WAIT_TIMEOUT 30 // wait 30 seconds before ending mission
220#endif
221
222static bool mission_wait_started = false;
223#if MISSION_WAIT_TIMEOUT
224static float mission_wait_time = 0.f;
225static struct _mission_element mission_wait_wp;
226static bool mission_wait_pattern(void) {
228 mission_wait_wp.element.mission_wp.wp = *stateGetPositionEnu_f();
229 mission_wait_time = 0.f;
231 }
232 mission_nav_wp(&mission_wait_wp);
233 mission_wait_time += dt_navigation;
234 return (mission_wait_time < (float)MISSION_WAIT_TIMEOUT); // keep flying until TIMEOUT
235}
236#else
237static bool mission_wait_pattern(void) {
238 return false; // no TIMEOUT, end mission now
239}
240#endif
241
243{
244 // current element
245 struct _mission_element *el = NULL;
246 if ((el = mission_get()) == NULL) {
247 return mission_wait_pattern();
248 }
249 mission_wait_started = false;
250
251 bool el_running = false;
252 switch (el->type) {
253 case MissionWP:
254 el_running = mission_nav_wp(el);
255 break;
256 case MissionCircle:
257 el_running = mission_nav_circle(el);
258 break;
259 case MissionSegment:
260 el_running = mission_nav_segment(el);
261 break;
262 case MissionPath:
263 el_running = mission_nav_path(el);
264 break;
265 case MissionCustom:
266 el_running = mission_nav_custom(&(el->element.mission_custom), mission.element_time < dt_navigation);
267 break;
268 default:
269 // invalid type or pattern not yet handled
270 break;
271 }
272
273 // increment element_time
274 mission.element_time += dt_navigation;
275
276 if (!el_running) {
277 // reset timer
278 mission.element_time = 0.;
279 // go to next element
280 mission.current_idx = (mission.current_idx + 1) % MISSION_ELEMENT_NB;
281 }
282 return true;
283}
284
Core autopilot interface common to all firmwares.
static const float dt_navigation
static float float_vect2_norm(const struct FloatVect2 *v)
#define VECT2_SMUL(_vo, _vi, _s)
#define VECT2_DIFF(_c, _a, _b)
#define VECT2_SUM(_c, _a, _b)
#define VECT3_SMUL(_vo, _vi, _s)
int32_t alt
in millimeters above WGS84 reference ellipsoid
void enu_of_lla_point_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla)
Convert a point from LLA to local ENU.
vector in East North Up coordinates
vector in Latitude, Longitude and Altitude
struct State state
Definition state.c:36
int32_t stateGetHmslOrigin_i(void)
Get the HMSL of the frame origin (int).
Definition state.c:196
struct LlaCoor_i stateGetLlaOrigin_i(void)
Get the LLA position of the frame origin (int).
Definition state.c:130
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:859
static struct LtpDef_i * stateGetNedOrigin_i(void)
Get the coordinate NED frame origin (int).
Definition state.h:567
struct _mission mission
struct _mission_element * mission_get(void)
Get current mission element.
mission planner library
float params[MISSION_CUSTOM_MAX]
list of parameters
struct _mission_registered * reg
pointer to a registered custom mission element
mission_custom_cb cb
navigation/action function callback
uint8_t nb
number of parameters
@ MissionCustom
@ MissionCircle
@ MissionWP
@ MissionSegment
@ MissionPath
@ MissionInit
first exec
@ MissionRun
normal run
union _mission_element::@136371027203076204005113273111204361211010126306 element
enum MissionType type
#define MISSION_ELEMENT_NB
Max number of elements in the tasks' list can be redefined.
float duration
time to spend in the element (<= 0 to disable)
#define MISSION_WAIT_TIMEOUT
Implement waiting pattern Only called when MISSION_WAIT_TIMEOUT is not 0.
static bool mission_wait_started
static bool mission_wait_pattern(void)
struct EnuCoor_f last_mission_wp
static bool mission_nav_wp(struct _mission_element *el)
Navigation function to a single waypoint.
static bool mission_nav_segment(struct _mission_element *el)
Navigation function along a segment.
static bool mission_nav_circle(struct _mission_element *el)
Navigation function on a circle.
static bool mission_nav_custom(struct _mission_custom *custom, bool init)
Call custom navigation function.
int mission_run()
Run mission.
bool mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla)
Utility function: converts lla (int) to local point (float).
#define BUFFER_ZONE_DIST
static bool mission_nav_path(struct _mission_element *el)
Navigation function along a path.
#define MISSION_PATH_SKIP_GOTO
float waypoint_get_x(uint8_t wp_id)
Get X/East coordinate of waypoint in meters.
Definition waypoints.c:124
float waypoint_get_y(uint8_t wp_id)
Get Y/North coordinate of waypoint in meters.
Definition waypoints.c:132
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
Definition nav.h:49
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition nav.h:191
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition nav.h:177
bool init
Definition nav_gls.c:57
float z
in meters
vector in East North Up coordinates Units: meters
struct RotorcraftNavigation nav
Definition navigation.c:51
bool nav_check_wp_time(struct EnuCoor_f *wp, uint16_t stay_time)
Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp.
Definition navigation.c:180
Rotorcraft navigation functions.
#define CARROT
default approaching_time for a wp
Definition navigation.h:70