Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
mission_fw_nav.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 Gautier Hattenberger
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 "modules/nav/common_nav.h"
35 #include "generated/flight_plan.h"
36 
38 bool mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla)
39 {
41  struct LlaCoor_f lla_f;
42  LLA_FLOAT_OF_BFP(lla_f, *lla);
43 
44  /* Computes from (lat, long) in the referenced UTM zone */
45  struct UtmCoor_f utm;
46  utm.zone = nav_utm_zone0;
47  utm_of_lla_f(&utm, &lla_f);
48 
49  /* Computes relative position to HOME waypoint
50  * and bound the distance to max_dist_from_home
51  */
52  float dx, dy;
53  dx = utm.east - nav_utm_east0 - waypoints[WP_HOME].x;
54  dy = utm.north - nav_utm_north0 - waypoints[WP_HOME].y;
55  BoundAbs(dx, max_dist_from_home);
56  BoundAbs(dy, max_dist_from_home);
57 
58  /* Update point */
59  point->x = waypoints[WP_HOME].x + dx;
60  point->y = waypoints[WP_HOME].y + dy;
61  point->z = lla_f.alt;
62 
63  return true;
64 }
65 
66 // navigation time step
67 static const float dt_navigation = 1.0f / ((float)NAVIGATION_FREQUENCY);
68 
69 // dirty hack to comply with nav_approaching_xy function
70 struct EnuCoor_f last_wp_f = { 0.f, 0.f, 0.f };
71 
74 static inline bool mission_nav_wp(struct _mission_wp *wp)
75 {
77 #ifdef MISSION_ALT_PROXIMITY
78  && fabsf(stateGetPositionEnu_f()->z - wp->wp.z) <= MISSION_ALT_PROXIMITY
79 #endif
80  ) {
81  last_wp_f = wp->wp; // store last wp
82  return false; // end of mission element
83  }
84  // set navigation command
85  fly_to_xy(wp->wp.x, wp->wp.y);
87  NavVerticalAltitudeMode(wp->wp.z, 0.f);
88  return true;
89 }
90 
93 static inline bool mission_nav_circle(struct _mission_circle *circle)
94 {
95  nav_circle_XY(circle->center.x, circle->center.y, circle->radius);
97  NavVerticalAltitudeMode(circle->center.z, 0.f);
98  return true;
99 }
100 
103 static inline bool mission_nav_segment(struct _mission_segment *segment)
104 {
105  if (nav_approaching_xy(segment->to.x, segment->to.y, segment->from.x, segment->from.y, CARROT)
106 #ifdef MISSION_ALT_PROXIMITY
107  && fabsf(stateGetPositionEnu_f()->z - segment->to.z) <= MISSION_ALT_PROXIMITY
108 #endif
109  ) {
110  last_wp_f = segment->to;
111  return false; // end of mission element
112  }
113  nav_route_xy(segment->from.x, segment->from.y, segment->to.x, segment->to.y);
115  NavVerticalAltitudeMode(segment->to.z, 0.f); // both altitude should be the same anyway
116  return true;
117 }
118 
121 static inline bool mission_nav_path(struct _mission_path *path)
122 {
123  if (path->nb == 0) {
124  return false; // nothing to do
125  }
126  if (path->nb == 1) {
127  // handle as a single waypoint
128  struct _mission_wp wp;
129  wp.wp = path->path[0];
130  return mission_nav_wp(&wp);
131  }
132  if (path->path_idx == path->nb - 1) {
133  last_wp_f = path->path[path->path_idx]; // store last wp
134  return false; // end of path
135  }
136  // normal case
137  struct EnuCoor_f from = path->path[path->path_idx];
138  struct EnuCoor_f to = path->path[path->path_idx + 1];
139  nav_route_xy(from.x, from.y, to.x, to.y);
141  NavVerticalAltitudeMode(to.z, 0.f); // both altitude should be the same anyway
142  if (nav_approaching_xy(to.x, to.y, from.x, from.y, CARROT)
143 #ifdef MISSION_ALT_PROXIMITY
144  && fabsf(stateGetPositionEnu_f()->z - to.z) <= MISSION_ALT_PROXIMITY
145 #endif
146  ) {
147  path->path_idx++; // go to next segment
148  }
149  return true;
150 }
151 
154 static inline bool mission_nav_custom(struct _mission_custom *custom, bool init)
155 {
156  if (init) {
157  return custom->reg->cb(custom->nb, custom->params, MissionInit);
158  } else {
159  return custom->reg->cb(custom->nb, custom->params, MissionRun);
160  }
161 }
162 
166 #ifndef MISSION_WAIT_TIMEOUT
167 #define MISSION_WAIT_TIMEOUT 120 // wait 2 minutes before ending mission
168 #endif
169 
170 static bool mission_wait_started = false;
171 #if MISSION_WAIT_TIMEOUT
172 static float mission_wait_time = 0.f;
174 static bool mission_wait_pattern(void) {
175  if (!mission_wait_started) {
179  mission_wait_time = 0.f;
180  mission_wait_started = true;
181  }
184  return (mission_wait_time < (float)MISSION_WAIT_TIMEOUT); // keep flying until TIMEOUT
185 }
186 #else
187 static bool mission_wait_pattern(void) {
188  return false; // no TIMEOUT, end mission now
189 }
190 #endif
191 
193 {
194  // current element
195  struct _mission_element *el = NULL;
196  if ((el = mission_get()) == NULL) {
197  return mission_wait_pattern();
198  }
199  mission_wait_started = false;
200 
201  bool el_running = false;
202  switch (el->type) {
203  case MissionWP:
204  el_running = mission_nav_wp(&(el->element.mission_wp));
205  break;
206  case MissionCircle:
207  el_running = mission_nav_circle(&(el->element.mission_circle));
208  break;
209  case MissionSegment:
210  el_running = mission_nav_segment(&(el->element.mission_segment));
211  break;
212  case MissionPath:
213  el_running = mission_nav_path(&(el->element.mission_path));
214  break;
215  case MissionCustom:
216  el_running = mission_nav_custom(&(el->element.mission_custom), mission.element_time < dt_navigation);
217  break;
218  default:
219  // invalid type or pattern not yet handled
220  break;
221  }
222 
223  // increment element_time
225 
226  // test if element is finished or element time is elapsed
227  if (!el_running || (el->duration > 0.f && mission.element_time >= el->duration)) {
228  // reset timer
229  mission.element_time = 0.f;
230  // go to next element
232  }
233 
234  return true;
235 }
236 
Core autopilot interface common to all firmwares.
int32_t nav_utm_east0
Definition: common_nav.c:43
uint8_t nav_utm_zone0
Definition: common_nav.c:45
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
int32_t nav_utm_north0
Definition: common_nav.c:44
float y
Definition: common_nav.h:41
float x
Definition: common_nav.h:40
#define LLA_FLOAT_OF_BFP(_o, _i)
vector in Latitude, Longitude and Altitude
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
struct _mission mission
struct _mission_element * mission_get(void)
Get current mission element.
mission planner library
float params[MISSION_CUSTOM_MAX]
list of parameters
union _mission_element::@299 element
struct EnuCoor_f wp
struct _mission_registered * reg
pointer to a registered custom mission element
mission_custom_cb cb
navigation/action function callback
struct EnuCoor_f to
uint8_t nb
number of parameters
@ MissionCustom
@ MissionCircle
@ MissionWP
@ MissionSegment
@ MissionPath
@ MissionInit
first exec
@ MissionRun
normal run
float element_time
time in second spend in the current element
struct EnuCoor_f from
enum MissionType type
#define MISSION_ELEMENT_NB
Max number of elements in the tasks' list can be redefined.
struct EnuCoor_f center
uint8_t path_idx
struct EnuCoor_f path[MISSION_PATH_NB]
float duration
time to spend in the element (<= 0 to disable)
uint8_t current_idx
current mission element index
static bool mission_wait_pattern(void)
static float mission_wait_time
static bool mission_nav_wp(struct _mission_wp *wp)
Navigation function to a single waypoint.
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)
static bool mission_nav_circle(struct _mission_circle *circle)
Navigation function on a circle.
static const float dt_navigation
static bool mission_nav_path(struct _mission_path *path)
Navigation function along a path.
#define MISSION_WAIT_TIMEOUT
Implement waiting pattern Only called when MISSION_WAIT_TIMEOUT is not 0.
static bool mission_nav_segment(struct _mission_segment *segment)
Navigation function along a segment.
struct EnuCoor_f last_wp_f
static struct _mission_circle mission_wait_circle
static bool mission_wait_started
bool nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time)
Decide if the UAV is approaching the current waypoint.
Definition: nav.c:325
void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y)
Computes the carrot position along the desired segment.
Definition: nav.c:382
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Definition: nav.c:108
void fly_to_xy(float x, float y)
Computes desired_x, desired_y and desired_course.
Definition: nav.c:356
Fixedwing Navigation library.
#define GetAltRef()
Get current altitude reference for local coordinates.
Definition: nav.h:242
#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
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
float alt
in meters (normally above WGS84 reference ellipsoid)
float y
in meters
float x
in meters
uint8_t zone
UTM zone number.
float east
in meters
float north
in meters
float z
in meters
vector in East North Up coordinates Units: meters
vector in Latitude, Longitude and Altitude
position in UTM coordinates Units: meters
const float max_dist_from_home
Definition: navigation.c:53
#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