Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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
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
45 return false;
46 }
47
48 // change geoid alt to ellipsoid alt
50
51 //Compute ENU components from LLA with respect to ltp origin
55 // result of enu_of_lla_point_i is in cm, convert to float in m
57
58 //Bound the new waypoint with max distance from home
59 struct FloatVect2 home;
64 //Saturate the mission wp not to overflow max_dist_from_home
65 //including a buffer zone before limits
70 }
71 // set new point
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
92#ifdef MISSION_ALT_PROXIMITY
94#endif
95 ) {
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
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);
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
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);
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
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);
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;
226static bool mission_wait_pattern(void) {
229 mission_wait_time = 0.f;
231 }
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:
255 break;
256 case MissionCircle:
258 break;
259 case MissionSegment:
261 break;
262 case MissionPath:
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
275
276 if (!el_running) {
277 // reset timer
279 // go to next element
281 }
282 return true;
283}
284
Core autopilot interface common to all firmwares.
static float float_vect2_norm(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
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition state.h:199
int32_t stateGetHmslOrigin_i(void)
Get the HMSL of the frame origin (int)
Definition state.c:190
struct LlaCoor_i stateGetLlaOrigin_i(void)
Get the LLA position of the frame origin (int)
Definition state.c:124
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:848
static struct LtpDef_i * stateGetNedOrigin_i(void)
Get the coordinate NED frame origin (int)
Definition state.h:556
uint16_t foo
Definition main_demo5.c:58
struct _mission mission
struct _mission_element * mission_get(void)
Get current mission element.
mission planner library
union _mission_element::@299 element
@ MissionCustom
@ MissionCircle
@ MissionWP
@ MissionSegment
@ MissionPath
@ MissionInit
first exec
@ MissionRun
normal run
float element_time
time in second spend in the current element
#define MISSION_ELEMENT_NB
Max number of elements in the tasks' list can be redefined.
uint8_t current_idx
current mission element index
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 const float dt_navigation
#define MISSION_WAIT_TIMEOUT
Implement waiting pattern Only called when MISSION_WAIT_TIMEOUT is not 0.
static bool mission_nav_path(struct _mission_element *el)
Navigation function along a path.
#define MISSION_PATH_SKIP_GOTO
static bool mission_wait_started
float waypoint_get_x(uint8_t wp_id)
Get X/East coordinate of waypoint in meters.
Definition waypoints.c:97
float waypoint_get_y(uint8_t wp_id)
Get Y/North coordinate of waypoint in meters.
Definition waypoints.c:105
#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.
navigation_approaching nav_approaching
Definition navigation.h:153
navigation_circle nav_circle
Definition navigation.h:154
#define CARROT
default approaching_time for a wp
Definition navigation.h:70
navigation_goto nav_goto
Definition navigation.h:151
navigation_route nav_route
Definition navigation.h:152