Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 
41 bool mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla)
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, &state.ned_origin_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 //Function that converts target wp from float point versions to int
80 {
81  struct _mission_element tmp_element = *el;
82  uint8_t i = 0;
83 
84  switch (tmp_element.type) {
85  case MissionWP:
86  ENU_BFP_OF_REAL(el->element.mission_wp.wp.wp_i, tmp_element.element.mission_wp.wp.wp_f);
87  break;
88  case MissionCircle:
89  ENU_BFP_OF_REAL(el->element.mission_circle.center.center_i, tmp_element.element.mission_circle.center.center_f);
90  break;
91  case MissionSegment:
92  ENU_BFP_OF_REAL(el->element.mission_segment.from.from_i, tmp_element.element.mission_segment.from.from_f);
93  ENU_BFP_OF_REAL(el->element.mission_segment.to.to_i, tmp_element.element.mission_segment.to.to_f);
94  break;
95  case MissionPath:
96  for (i = 0; i < 5; i++) {
97  ENU_BFP_OF_REAL(el->element.mission_path.path.path_i[i], tmp_element.element.mission_path.path.path_f[i]);
98  }
99  break;
100  default:
101  // invalid element type
102  return false;
103  break;
104  }
105 
106  return true;
107 }
108 
109 // navigation time step
110 static const float dt_navigation = 1.0 / ((float)NAV_FREQ);
111 
112 // last_mission_wp, last target wp from mission elements, not used actively and kept for future implementations
113 struct EnuCoor_i last_mission_wp = { 0., 0., 0. };
114 
117 static inline bool mission_nav_wp(struct _mission_element *el)
118 {
119  struct EnuCoor_i *target_wp = &(el->element.mission_wp.wp.wp_i);
120 
121  //Check proximity and wait for 'duration' seconds in proximity circle if desired
122  if (nav_approaching_from(target_wp, NULL, CARROT)) {
123  last_mission_wp = *target_wp;
124 
125  if (el->duration > 0.) {
126  if (nav_check_wp_time(target_wp, el->duration)) { return false; }
127  } else { return false; }
128 
129  }
130  //Go to Mission Waypoint
132  VECT3_COPY(navigation_target, *target_wp);
133  NavVerticalAutoThrottleMode(RadOfDeg(0.000000));
134  NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(target_wp->z), 0.);
135 
136  return true;
137 }
138 
141 static inline bool mission_nav_circle(struct _mission_element *el)
142 {
143  struct EnuCoor_i *center_wp = &(el->element.mission_circle.center.center_i);
144  int32_t radius = el->element.mission_circle.radius;
145 
146  //Draw the desired circle for a 'duration' time
148  nav_circle(center_wp, POS_BFP_OF_REAL(radius));
149  NavVerticalAutoThrottleMode(RadOfDeg(0.0));
150  NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(center_wp->z), 0.);
151 
152  if (el->duration > 0. && mission.element_time >= el->duration) {
153  return false;
154  }
155 
156  return true;
157 }
158 
161 static inline bool mission_nav_segment(struct _mission_element *el)
162 {
163  struct EnuCoor_i *from_wp = &(el->element.mission_segment.from.from_i);
164  struct EnuCoor_i *to_wp = &(el->element.mission_segment.to.to_i);
165 
166  //Check proximity and wait for 'duration' seconds in proximity circle if desired
167  if (nav_approaching_from(to_wp, from_wp, CARROT)) {
168  last_mission_wp = *to_wp;
169 
170  if (el->duration > 0.) {
171  if (nav_check_wp_time(to_wp, el->duration)) { return false; }
172  } else { return false; }
173  }
174 
175  //Route Between from-to
177  nav_route(from_wp, to_wp);
178  NavVerticalAutoThrottleMode(RadOfDeg(0.0));
180 
181  return true;
182 }
183 
184 
187 static inline bool mission_nav_path(struct _mission_element *el)
188 {
189  if (el->element.mission_path.nb == 0) {
190  return false; // nothing to do
191  }
192 
193  if (el->element.mission_path.path_idx == 0) { //first wp of path
194  el->element.mission_wp.wp.wp_i = el->element.mission_path.path.path_i[0];
195  if (!mission_nav_wp(el)) { el->element.mission_path.path_idx++; }
196  }
197 
198  else if (el->element.mission_path.path_idx < el->element.mission_path.nb) { //standart wp of path
199 
200  struct EnuCoor_i *from_wp = &(el->element.mission_path.path.path_i[(el->element.mission_path.path_idx) - 1]);
201  struct EnuCoor_i *to_wp = &(el->element.mission_path.path.path_i[el->element.mission_path.path_idx]);
202 
203  //Check proximity and wait for t seconds in proximity circle if desired
204  if (nav_approaching_from(to_wp, from_wp, CARROT)) {
205  last_mission_wp = *to_wp;
206 
207  if (el->duration > 0.) {
208  if (nav_check_wp_time(to_wp, el->duration)) {
209  el->element.mission_path.path_idx++;
210  }
211  } else { el->element.mission_path.path_idx++; }
212  }
213  //Route Between from-to
215  nav_route(from_wp, to_wp);
216  NavVerticalAutoThrottleMode(RadOfDeg(0.0));
218  } else { return false; } //end of path
219 
220  return true;
221 }
222 
225 static inline bool mission_nav_custom(struct _mission_custom *custom, bool init)
226 {
227  return custom->reg->cb(custom->nb, custom->params, init);
228 }
229 
231 {
232  // current element
233  struct _mission_element *el = NULL;
234  if ((el = mission_get()) == NULL) {
235  mission.element_time = 0;
236  mission.current_idx = 0;
237  return false; // end of mission
238  }
239 
240  bool el_running = false;
241  switch (el->type) {
242  case MissionWP:
243  el_running = mission_nav_wp(el);
244  break;
245  case MissionCircle:
246  el_running = mission_nav_circle(el);
247  break;
248  case MissionSegment:
249  el_running = mission_nav_segment(el);
250  break;
251  case MissionPath:
252  el_running = mission_nav_path(el);
253  break;
254  case MissionCustom:
255  el_running = mission_nav_custom(&(el->element.mission_custom), mission.element_time < dt_navigation);
256  break;
257  default:
258  // invalid type or pattern not yet handled
259  break;
260  }
261 
262  // increment element_time
264 
265  if (!el_running) {
266  // reset timer
267  mission.element_time = 0.;
268  // go to next element
270  }
271  return true;
272 }
273 
_mission_element::element
union _mission_element::@299 element
_mission_element::type
enum MissionType type
Definition: mission_common.h:115
VECT2_SMUL
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:98
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
MissionCircle
@ MissionCircle
Definition: mission_common.h:39
mission_nav_circle
static bool mission_nav_circle(struct _mission_element *el)
Navigation function on a circle.
Definition: mission_rotorcraft_nav.c:141
VECT3_SMUL
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:189
State::ned_origin_i
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
_mission::current_idx
uint8_t current_idx
current mission element index
Definition: mission_common.h:147
_mission_element
Definition: mission_common.h:114
dt_navigation
static const float dt_navigation
Definition: mission_rotorcraft_nav.c:110
mission
struct _mission mission
Definition: mission_common.c:35
mission_nav_wp
static bool mission_nav_wp(struct _mission_element *el)
Navigation function to a single waypoint.
Definition: mission_rotorcraft_nav.c:117
_mission_custom
Definition: mission_common.h:108
mission_point_of_lla
bool mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla)
Utility function: converts lla (int) to local point (float)
Definition: mission_rotorcraft_nav.c:41
nav_check_wp_time
bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time)
Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp
Definition: navigation.c:306
BUFFER_ZONE_DIST
#define BUFFER_ZONE_DIST
Definition: mission_rotorcraft_nav.c:38
nav_approaching_from
bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time)
Proximity tests on approaching a wp.
Definition: navigation.c:262
nav_circle
void nav_circle(struct EnuCoor_i *wp_center, int32_t radius)
Definition: navigation.c:543
navigation_target
struct EnuCoor_i navigation_target
Definition: navigation.c:91
MissionCustom
@ MissionCustom
Definition: mission_common.h:42
POS_FLOAT_OF_BFP
#define POS_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:217
waypoint_get_y
float waypoint_get_y(uint8_t wp_id)
Get Y/North coordinate of waypoint in meters.
Definition: waypoints.c:85
point
Definition: common_nav.h:39
mission_element_convert
bool mission_element_convert(struct _mission_element *el)
Convert mission element's points format if needed.
Definition: mission_rotorcraft_nav.c:79
State::ned_initialized_i
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
EnuCoor_f::z
float z
in meters
Definition: pprz_geodetic_float.h:75
NavVerticalAutoThrottleMode
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition: nav.h:171
NAV_FREQ
#define NAV_FREQ
Definition: navigation.h:43
VECT2_DIFF
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
HORIZONTAL_MODE_WAYPOINT
#define HORIZONTAL_MODE_WAYPOINT
Definition: nav.h:85
CARROT
#define CARROT
default approaching_time for a wp
Definition: navigation.h:40
FloatVect2
Definition: pprz_algebra_float.h:49
MissionWP
@ MissionWP
Definition: mission_common.h:38
mission_nav_custom
static bool mission_nav_custom(struct _mission_custom *custom, bool init)
Call custom navigation function.
Definition: mission_rotorcraft_nav.c:225
float_vect2_norm
static float float_vect2_norm(struct FloatVect2 *v)
Definition: pprz_algebra_float.h:139
enu_of_lla_point_i
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.
Definition: pprz_geodetic_int.c:295
waypoint_get_x
float waypoint_get_x(uint8_t wp_id)
Get X/East coordinate of waypoint in meters.
Definition: waypoints.c:77
HORIZONTAL_MODE_CIRCLE
#define HORIZONTAL_MODE_CIRCLE
Definition: nav.h:87
nav_route
void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
Definition: navigation.c:589
uint8_t
unsigned char uint8_t
Definition: types.h:14
last_mission_wp
struct EnuCoor_i last_mission_wp
Definition: mission_rotorcraft_nav.c:113
EnuCoor_f
vector in East North Up coordinates Units: meters
Definition: pprz_geodetic_float.h:72
mission_run
int mission_run()
Run mission.
Definition: mission_rotorcraft_nav.c:230
autopilot.h
_mission_registered::cb
mission_custom_cb cb
navigation/action function callback
Definition: mission_common.h:104
_mission_custom::reg
struct _mission_registered * reg
pointer to a registered custom mission element
Definition: mission_common.h:109
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
MISSION_ELEMENT_NB
#define MISSION_ELEMENT_NB
Max number of elements in the tasks' list can be redefined.
Definition: mission_common.h:132
init
bool init
Definition: nav_gls.c:57
mission_common.h
mission planner library
_mission_custom::params
float params[MISSION_CUSTOM_MAX]
list of parameters
Definition: mission_common.h:110
LlaCoor_i
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_int.h:59
LtpDef_i::hmsl
int32_t hmsl
Height above mean sea level in mm.
Definition: pprz_geodetic_int.h:102
int32_t
signed long int32_t
Definition: types.h:19
VECT2_SUM
#define VECT2_SUM(_c, _a, _b)
Definition: pprz_algebra.h:86
navigation.h
mission_get
struct _mission_element * mission_get(void)
Get current mission element.
Definition: mission_common.c:130
MissionPath
@ MissionPath
Definition: mission_common.h:41
POS_BFP_OF_REAL
#define POS_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:216
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
_mission_custom::nb
uint8_t nb
number of parameters
Definition: mission_common.h:111
EnuCoor_i::z
int32_t z
Up.
Definition: pprz_geodetic_int.h:80
mission_nav_segment
static bool mission_nav_segment(struct _mission_element *el)
Navigation function along a segment.
Definition: mission_rotorcraft_nav.c:161
_mission_element::duration
float duration
time to spend in the element (<= 0 to disable)
Definition: mission_common.h:124
HORIZONTAL_MODE_ROUTE
#define HORIZONTAL_MODE_ROUTE
Definition: nav.h:86
MissionSegment
@ MissionSegment
Definition: mission_common.h:40
state
struct State state
Definition: state.c:36
LtpDef_i::lla
struct LlaCoor_i lla
Reference point in lla.
Definition: pprz_geodetic_int.h:100
horizontal_mode
uint8_t horizontal_mode
Definition: nav.c:71
EnuCoor_i
vector in East North Up coordinates
Definition: pprz_geodetic_int.h:77
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
NavVerticalAltitudeMode
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition: nav.h:185
mission_nav_path
static bool mission_nav_path(struct _mission_element *el)
Navigation function along a path.
Definition: mission_rotorcraft_nav.c:187
ENU_BFP_OF_REAL
#define ENU_BFP_OF_REAL(_o, _i)
Definition: pprz_geodetic_int.h:194
_mission::element_time
float element_time
time in second spend in the current element
Definition: mission_common.h:145