Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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>
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 
40 // Utility function: converts lla to local point
41 bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_f *lla) {
42  // return FALSE if there is no valid local coordinate
43  // FIXME we should only test if local frame is initialized, not valid
44  if (!stateIsLocalCoordinateValid()) return FALSE;
45 
46  // change geoid alt to ellipsoid alt
48  //Compute ENU components from LLA with respect to ltp origin
49  struct EnuCoor_f tmp_enu_point;
50  enu_of_lla_point_f(&tmp_enu_point, &state.ned_origin_f, lla);
51 
52  //Bound the new waypoint with max distance from home
53  struct EnuCoor_f home;
54  ENU_FLOAT_OF_BFP(home, waypoints[WP_HOME]);
55  struct FloatVect2 vect_from_home;
56  FLOAT_VECT2_DIFF(vect_from_home, tmp_enu_point, home);
57  //Saturate the mission wp not to overflow max_dist_from_home
58  //including a buffer zone before limits
59  float dist_to_home;
60  FLOAT_VECT2_NORM(dist_to_home, vect_from_home);
61  dist_to_home += BUFFER_ZONE_DIST;
62  if (dist_to_home > MAX_DIST_FROM_HOME) {
63  FLOAT_VECT2_SMUL(vect_from_home, vect_from_home, (MAX_DIST_FROM_HOME / dist_to_home));
64  }
65  // set new point
66  FLOAT_VECT2_SUM(*point, home, vect_from_home);
67  point->z = tmp_enu_point.z;
68 
69  return TRUE;
70 }
71 
72 //Function that converts target wp from float point versions to int
74  struct _mission_element tmp_element = *el;
75  uint8_t i = 0;
76 
77  switch (tmp_element.type) {
78  case MissionWP:
80  break;
81  case MissionCircle:
83  break;
84  case MissionSegment:
87  break;
88  case MissionPath:
89  for (i = 0; i < 5; i++) {
91  }
92  break;
93  default:
94  // invalid element type
95  return FALSE;
96  break;
97  }
98 
99  return TRUE;
100 }
101 
102 // navigation time step
103 const float dt_navigation = 1.0 / ((float)NAV_FREQ);
104 
105 // last_mission_wp, last target wp from mission elements, not used actively and kept for future implementations
106 struct EnuCoor_i last_mission_wp = { 0., 0., 0. };
107 
110 static inline bool_t mission_nav_wp(struct _mission_element * el) {
111  struct EnuCoor_i* target_wp = &(el->element.mission_wp.wp.wp_i);
112 
113  //Check proximity and wait for 'duration' seconds in proximity circle if desired
114  if (nav_approaching_from(target_wp, NULL, CARROT)) {
115  last_mission_wp = *target_wp;
116 
117  if (el->duration > 0.) {
118  if (nav_check_wp_time(target_wp, el->duration)) return FALSE;
119  }
120  else return FALSE;
121 
122  }
123  //Go to Mission Waypoint
125  INT32_VECT3_COPY(navigation_target, *target_wp);
126  NavVerticalAutoThrottleMode(RadOfDeg(0.000000));
127  NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(target_wp->z), 0.);
128 
129  return TRUE;
130 }
131 
134 static inline bool_t mission_nav_circle(struct _mission_element * el) {
135  struct EnuCoor_i* center_wp = &(el->element.mission_circle.center.center_i);
137 
138  //Draw the desired circle for a 'duration' time
140  nav_circle(center_wp, POS_BFP_OF_REAL(radius));
141  NavVerticalAutoThrottleMode(RadOfDeg(0.0));
142  NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(center_wp->z), 0.);
143 
144  if (el->duration > 0. && mission.element_time >= el->duration) {
145  return FALSE;
146  }
147 
148  return TRUE;
149 }
150 
153 static inline bool_t mission_nav_segment(struct _mission_element * el) {
154  struct EnuCoor_i* from_wp = &(el->element.mission_segment.from.from_i);
155  struct EnuCoor_i* to_wp = &(el->element.mission_segment.to.to_i);
156 
157  //Check proximity and wait for 'duration' seconds in proximity circle if desired
158  if (nav_approaching_from(to_wp, from_wp, CARROT)) {
159  last_mission_wp = *to_wp;
160 
161  if (el->duration > 0.) {
162  if (nav_check_wp_time(to_wp, el->duration)) return FALSE;
163  }
164  else return FALSE;
165  }
166 
167  //Route Between from-to
169  nav_route(from_wp, to_wp);
170  NavVerticalAutoThrottleMode(RadOfDeg(0.0));
172 
173  return TRUE;
174 }
175 
176 
179 static inline bool_t mission_nav_path(struct _mission_element * el) {
180  if (el->element.mission_path.nb == 0) {
181  return FALSE; // nothing to do
182  }
183 
184  if (el->element.mission_path.path_idx == 0) { //first wp of path
187  }
188 
189  else if (el->element.mission_path.path_idx < el->element.mission_path.nb) { //standart wp of path
190 
191  struct EnuCoor_i* from_wp = &(el->element.mission_path.path.path_i[(el->element.mission_path.path_idx) - 1]);
193 
194  //Check proximity and wait for t seconds in proximity circle if desired
195  if (nav_approaching_from(to_wp, from_wp, CARROT)) {
196  last_mission_wp = *to_wp;
197 
198  if (el->duration > 0.) {
199  if (nav_check_wp_time(to_wp, el->duration))
201  }
202  else el->element.mission_path.path_idx++;
203  }
204  //Route Between from-to
206  nav_route(from_wp, to_wp);
207  NavVerticalAutoThrottleMode(RadOfDeg(0.0));
209  }
210  else return FALSE; //end of path
211 
212  return TRUE;
213 }
214 
215 int mission_run() {
216  // current element
217  struct _mission_element * el = NULL;
218  if ((el = mission_get()) == NULL) {
219  mission.element_time = 0;
220  mission.current_idx = 0;
221  return FALSE; // end of mission
222  }
223 
224  bool_t el_running = FALSE;
225  switch (el->type){
226  case MissionWP:
227  el_running = mission_nav_wp(el);
228  break;
229  case MissionCircle:
230  el_running = mission_nav_circle(el);
231  break;
232  case MissionSegment:
233  el_running = mission_nav_segment(el);
234  break;
235  case MissionPath:
236  el_running = mission_nav_path(el);
237  break;
238  default:
239  // invalid type or pattern not yet handled
240  break;
241  }
242 
243  // increment element_time
245 
246  if (!el_running) {
247  // reset timer
248  mission.element_time = 0.;
249  // go to next element
251  }
252  return TRUE;
253 }
254 
static bool_t mission_nav_wp(struct _mission_element *el)
Navigation function to a single waypoint.
int mission_run()
Run mission.
struct EnuCoor_f path_f[MISSION_PATH_NB]
struct EnuCoor_i last_mission_wp
#define FLOAT_VECT2_DIFF(_c, _a, _b)
enum MissionType type
union _mission_path::@17 path
struct EnuCoor_f from_f
struct _mission_element * mission_get(void)
Get current mission element.
static bool_t mission_nav_segment(struct _mission_element *el)
Navigation function along a segment.
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:216
static float radius
Definition: chemotaxis.c:15
union _mission_wp::@13 wp
float z
in meters
static bool_t mission_nav_circle(struct _mission_element *el)
Navigation function on a circle.
uint8_t current_idx
current mission element index
Autopilot modes.
void enu_of_lla_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct LlaCoor_f *lla)
struct _mission_wp mission_wp
union _mission_circle::@14 center
#define FLOAT_VECT2_SMUL(_vo, _vi, _s)
bool_t mission_element_convert(struct _mission_element *el)
Convert mission element's points format if needed.
vector in Latitude, Longitude and Altitude
struct _mission_segment mission_segment
#define FALSE
Definition: imu_chimu.h:141
#define ENU_BFP_OF_REAL(_o, _i)
#define FLOAT_VECT2_SUM(_c, _a, _b)
#define BUFFER_ZONE_DIST
int32_t z
Up.
struct EnuCoor_i to_i
struct EnuCoor_i center_i
#define MISSION_ELEMENT_NB
Max number of elements in the tasks' list can be redefined.
struct EnuCoor_i wp_i
mission planner library
union _mission_segment::@15 from
#define POS_FLOAT_OF_BFP(_ai)
static bool_t stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
Definition: state.h:489
struct _mission_path mission_path
struct _mission_circle mission_circle
float element_time
time in second spend in the current element
#define INT32_VECT3_COPY(_o, _i)
float alt
in meters above WGS84 reference ellipsoid
static bool_t mission_nav_path(struct _mission_element *el)
Navigation function along a path.
signed long int32_t
Definition: types.h:19
#define TRUE
Definition: imu_chimu.h:144
uint8_t path_idx
struct EnuCoor_i from_i
bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_f *lla)
Get the ENU component of LLA mission point This function is firmware specific.
unsigned char uint8_t
Definition: types.h:14
#define FLOAT_VECT2_NORM(_n, _v)
struct EnuCoor_i path_i[MISSION_PATH_NB]
struct LlaCoor_f lla
origin of local frame in LLA
struct _mission mission
#define ENU_FLOAT_OF_BFP(_o, _i)
union _mission_segment::@16 to
struct EnuCoor_f to_f
vector in East North Up coordinates Units: meters
struct EnuCoor_f wp_f
union _mission_element::@18 element
const float dt_navigation
struct EnuCoor_f center_f
vector in East North Up coordinates
float hmsl
Height above mean sea level in meters.
struct State state
Definition: state.c:36
float duration
time to spend in the element (<= 0 to disable)
#define POS_BFP_OF_REAL(_af)