Paparazzi UAS  v6.2_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.0 / ((float)NAVIGATION_FREQUENCY);
68 
69 // dirty hack to comply with nav_approaching_xy function
70 struct EnuCoor_f last_wp_f = { 0., 0., 0. };
71 
74 static inline bool mission_nav_wp(struct _mission_wp *wp)
75 {
76  if (nav_approaching_xy(wp->wp.wp_f.x, wp->wp.wp_f.y, last_wp_f.x, last_wp_f.y, CARROT)) {
77  last_wp_f = wp->wp.wp_f; // store last wp
78  return false; // end of mission element
79  }
80  // set navigation command
81  fly_to_xy(wp->wp.wp_f.x, wp->wp.wp_f.y);
83  NavVerticalAltitudeMode(wp->wp.wp_f.z, 0.);
84  return true;
85 }
86 
89 static inline bool mission_nav_circle(struct _mission_circle *circle)
90 {
91  nav_circle_XY(circle->center.center_f.x, circle->center.center_f.y, circle->radius);
93  NavVerticalAltitudeMode(circle->center.center_f.z, 0.);
94  return true;
95 }
96 
99 static inline bool mission_nav_segment(struct _mission_segment *segment)
100 {
101  if (nav_approaching_xy(segment->to.to_f.x, segment->to.to_f.y, segment->from.from_f.x, segment->from.from_f.y,
102  CARROT)) {
103  last_wp_f = segment->to.to_f;
104  return false; // end of mission element
105  }
106  nav_route_xy(segment->from.from_f.x, segment->from.from_f.y, segment->to.to_f.x, segment->to.to_f.y);
108  NavVerticalAltitudeMode(segment->to.to_f.z, 0.); // both altitude should be the same anyway
109  return true;
110 }
111 
114 static inline bool mission_nav_path(struct _mission_path *path)
115 {
116  if (path->nb == 0) {
117  return false; // nothing to do
118  }
119  if (path->nb == 1) {
120  // handle as a single waypoint
121  struct _mission_wp wp;
122  wp.wp.wp_f = path->path.path_f[0];
123  return mission_nav_wp(&wp);
124  }
125  if (path->path_idx == path->nb - 1) {
126  last_wp_f = path->path.path_f[path->path_idx]; // store last wp
127  return false; // end of path
128  }
129  // normal case
130  struct EnuCoor_f from_f = path->path.path_f[path->path_idx];
131  struct EnuCoor_f to_f = path->path.path_f[path->path_idx + 1];
132  nav_route_xy(from_f.x, from_f.y, to_f.x, to_f.y);
134  NavVerticalAltitudeMode(to_f.z, 0.); // both altitude should be the same anyway
135  if (nav_approaching_xy(to_f.x, to_f.y, from_f.x, from_f.y, CARROT)) {
136  path->path_idx++; // go to next segment
137  }
138  return true;
139 }
140 
143 static inline bool mission_nav_custom(struct _mission_custom *custom, bool init)
144 {
145  if (init) {
146  return custom->reg->cb(custom->nb, custom->params, MissionInit);
147  } else {
148  return custom->reg->cb(custom->nb, custom->params, MissionRun);
149  }
150 }
151 
155 #ifndef MISSION_WAIT_TIMEOUT
156 #define MISSION_WAIT_TIMEOUT 120 // wait 2 minutes before ending mission
157 #endif
158 
159 static bool mission_wait_started = false;
160 #if MISSION_WAIT_TIMEOUT
161 static float mission_wait_time = 0.f;
163 static bool mission_wait_pattern(void) {
164  if (!mission_wait_started) {
166  mission_wait_circle.center.center_f.z += GetAltRef();
168  mission_wait_time = 0.f;
169  mission_wait_started = true;
170  }
173  return (mission_wait_time < (float)MISSION_WAIT_TIMEOUT); // keep flying until TIMEOUT
174 }
175 #else
176 static bool mission_wait_pattern(void) {
177  return false; // no TIMEOUT, end mission now
178 }
179 #endif
180 
182 {
183  // current element
184  struct _mission_element *el = NULL;
185  if ((el = mission_get()) == NULL) {
186  return mission_wait_pattern();
187  }
188  mission_wait_started = false;
189 
190  bool el_running = false;
191  switch (el->type) {
192  case MissionWP:
193  el_running = mission_nav_wp(&(el->element.mission_wp));
194  break;
195  case MissionCircle:
196  el_running = mission_nav_circle(&(el->element.mission_circle));
197  break;
198  case MissionSegment:
199  el_running = mission_nav_segment(&(el->element.mission_segment));
200  break;
201  case MissionPath:
202  el_running = mission_nav_path(&(el->element.mission_path));
203  break;
204  case MissionCustom:
205  el_running = mission_nav_custom(&(el->element.mission_custom), mission.element_time < dt_navigation);
206  break;
207  default:
208  // invalid type or pattern not yet handled
209  break;
210  }
211 
212  // increment element_time
214 
215  // test if element is finished or element time is elapsed
216  if (!el_running || (el->duration > 0. && mission.element_time >= el->duration)) {
217  // reset timer
218  mission.element_time = 0.;
219  // go to next element
221  }
222 
223  return true;
224 }
mission_wait_circle
static struct _mission_circle mission_wait_circle
Definition: mission_fw_nav.c:162
_mission_element::type
enum MissionType type
Definition: mission_common.h:121
mission_wait_time
static float mission_wait_time
Definition: mission_fw_nav.c:161
MissionCircle
@ MissionCircle
Definition: mission_common.h:39
UtmCoor_f::north
float north
in meters
Definition: pprz_geodetic_float.h:82
_mission::current_idx
uint8_t current_idx
current mission element index
Definition: mission_common.h:153
_mission_element
Definition: mission_common.h:120
nav_utm_east0
int32_t nav_utm_east0
Definition: common_nav.c:43
mission
struct _mission mission
Definition: mission_common.c:40
_mission_segment::to
union _mission_segment::@286 to
mission_wait_started
static bool mission_wait_started
Definition: mission_fw_nav.c:159
MissionInit
@ MissionInit
first exec
Definition: mission_common.h:55
last_wp_f
struct EnuCoor_f last_wp_f
Definition: mission_fw_nav.c:70
_mission_custom
Definition: mission_common.h:114
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
UtmCoor_f::east
float east
in meters
Definition: pprz_geodetic_float.h:83
nav_circle_XY
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Definition: nav.c:111
MissionCustom
@ MissionCustom
Definition: mission_common.h:42
common_nav.h
LLA_FLOAT_OF_BFP
#define LLA_FLOAT_OF_BFP(_o, _i)
Definition: pprz_geodetic_int.h:182
point
Definition: common_nav.h:39
waypoints
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
mission_nav_segment
static bool mission_nav_segment(struct _mission_segment *segment)
Navigation function along a segment.
Definition: mission_fw_nav.c:99
nav_utm_north0
int32_t nav_utm_north0
Definition: common_nav.c:44
mission_nav_wp
static bool mission_nav_wp(struct _mission_wp *wp)
Navigation function to a single waypoint.
Definition: mission_fw_nav.c:74
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_fw_nav.c:38
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
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:180
GetAltRef
#define GetAltRef()
Get current altitude reference for local coordinates.
Definition: nav.h:246
utm_of_lla_f
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
Definition: pprz_geodetic_float.c:308
mission_nav_custom
static bool mission_nav_custom(struct _mission_custom *custom, bool init)
Call custom navigation function.
Definition: mission_fw_nav.c:143
CARROT
#define CARROT
default approaching_time for a wp
Definition: navigation.h:40
MissionWP
@ MissionWP
Definition: mission_common.h:38
nav_route_xy
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:383
DEFAULT_CIRCLE_RADIUS
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
Definition: navigation.c:56
_mission_circle::radius
float radius
Definition: mission_common.h:72
UtmCoor_f::zone
uint8_t zone
UTM zone number.
Definition: pprz_geodetic_float.h:85
_mission_path::path
union _mission_path::@287 path
_mission_circle::center
union _mission_circle::@284 center
NAVIGATION_FREQUENCY
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
Definition: nav.h:49
_mission_wp
Definition: mission_common.h:59
point::x
float x
Definition: common_nav.h:40
MissionRun
@ MissionRun
normal run
Definition: mission_common.h:54
_mission_element::element
union _mission_element::@288 element
EnuCoor_f
vector in East North Up coordinates Units: meters
Definition: pprz_geodetic_float.h:72
mission_nav_path
static bool mission_nav_path(struct _mission_path *path)
Navigation function along a path.
Definition: mission_fw_nav.c:114
MISSION_WAIT_TIMEOUT
#define MISSION_WAIT_TIMEOUT
Implement waiting pattern Only called when MISSION_WAIT_TIMEOUT is not 0.
Definition: mission_fw_nav.c:156
autopilot.h
_mission_registered::cb
mission_custom_cb cb
navigation/action function callback
Definition: mission_common.h:110
LlaCoor_f::alt
float alt
in meters (normally above WGS84 reference ellipsoid)
Definition: pprz_geodetic_float.h:57
_mission_custom::reg
struct _mission_registered * reg
pointer to a registered custom mission element
Definition: mission_common.h:115
MISSION_ELEMENT_NB
#define MISSION_ELEMENT_NB
Max number of elements in the tasks' list can be redefined.
Definition: mission_common.h:138
mission_wait_pattern
static bool mission_wait_pattern(void)
Definition: mission_fw_nav.c:163
init
bool init
Definition: nav_gls.c:57
_mission_segment::from
union _mission_segment::@285 from
nav.h
mission_common.h
mission planner library
_mission_custom::params
float params[MISSION_CUSTOM_MAX]
list of parameters
Definition: mission_common.h:116
mission_run
int mission_run()
Run mission.
Definition: mission_fw_nav.c:181
_mission_wp::wp
union _mission_wp::@283 wp
_mission_segment
Definition: mission_common.h:75
LlaCoor_i
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_int.h:59
_mission_path
Definition: mission_common.h:88
nav_approaching_xy
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:326
UtmCoor_f
position in UTM coordinates Units: meters
Definition: pprz_geodetic_float.h:81
nav_utm_zone0
uint8_t nav_utm_zone0
Definition: common_nav.c:45
point::y
float y
Definition: common_nav.h:41
mission_nav_circle
static bool mission_nav_circle(struct _mission_circle *circle)
Navigation function on a circle.
Definition: mission_fw_nav.c:89
mission_get
struct _mission_element * mission_get(void)
Get current mission element.
Definition: mission_common.c:162
_mission_path::nb
uint8_t nb
Definition: mission_common.h:95
MissionPath
@ MissionPath
Definition: mission_common.h:41
_mission_custom::nb
uint8_t nb
number of parameters
Definition: mission_common.h:117
EnuCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:73
_mission_element::duration
float duration
time to spend in the element (<= 0 to disable)
Definition: mission_common.h:130
_mission_circle
Definition: mission_common.h:66
MissionSegment
@ MissionSegment
Definition: mission_common.h:40
_mission_path::path_idx
uint8_t path_idx
Definition: mission_common.h:94
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:194
dt_navigation
static const float dt_navigation
Definition: mission_fw_nav.c:67
max_dist_from_home
const float max_dist_from_home
Definition: navigation.c:88
_mission::element_time
float element_time
time in second spend in the current element
Definition: mission_common.h:151
LlaCoor_f
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_float.h:54
fly_to_xy
void fly_to_xy(float x, float y)
Computes desired_x, desired_y and desired_course.
Definition: nav.c:357