PX4 Firmware
PX4 Autopilot Software http://px4.io
navigation.h
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2012 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file navigation.h
36  * Definition of a mission consisting of mission items.
37  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
38  * @author Julian Oes <joes@student.ethz.ch>
39  * @author Lorenz Meier <lm@inf.ethz.ch>
40  */
41 
42 #pragma once
43 
44 #include <stdint.h>
45 #include <stdbool.h>
46 
47 #if defined(MEMORY_CONSTRAINED_SYSTEM)
48 # define NUM_MISSIONS_SUPPORTED 50
49 #elif defined(__PX4_POSIX)
50 # define NUM_MISSIONS_SUPPORTED (UINT16_MAX-1) // This is allocated as needed.
51 #else
52 # define NUM_MISSIONS_SUPPORTED 2000 // This allocates a file of around 181 kB on the SD card.
53 #endif
54 
55 #define NAV_EPSILON_POSITION 0.001f /**< Anything smaller than this is considered zero */
56 
57 /* compatible to mavlink MAV_CMD */
58 enum NAV_CMD {
97  NAV_CMD_INVALID = UINT16_MAX /* ensure that casting a large number results in a specific error */
98 };
99 
100 enum ORIGIN {
103 };
104 
105 /* compatible to mavlink MAV_FRAME */
106 enum NAV_FRAME {
119 };
120 
121 /**
122  * @addtogroup topics
123  * @{
124  */
125 
126 /**
127  * Global position setpoint in WGS84 coordinates.
128  *
129  * This is the position the MAV is heading towards. If it is of type loiter,
130  * the MAV is circling around it with the given loiter radius in meters.
131  *
132  * Corresponds to one of the DM_KEY_WAYPOINTS_OFFBOARD_* dataman items
133  */
134 
135 // Mission Item structure
136 // We explicitly handle struct padding to ensure consistency between in memory and on disk formats across different platforms, toolchains, etc
137 // The use of #pragma pack is avoided to prevent the possibility of unaligned memory accesses.
138 
139 #if (__GNUC__ >= 5) || __clang__
140 // Disabled in GCC 4.X as the warning doesn't seem to "pop" correctly
141 #pragma GCC diagnostic push
142 #pragma GCC diagnostic error "-Wpadded"
143 #endif // GCC >= 5 || Clang
144 
146  double lat; /**< latitude in degrees */
147  double lon; /**< longitude in degrees */
148  union {
149  struct {
150  union {
151  float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */
152  float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */
153  float circle_radius; /**< geofence circle radius in meters (only used for NAV_CMD_NAV_FENCE_CIRCLE*) */
154  };
155  float acceptance_radius; /**< default radius in which the mission is accepted as reached in meters */
156  float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover, negative for counter-clockwise */
157  float yaw; /**< in radians NED -PI..+PI, NAN means don't change yaw */
158  float ___lat_float; /**< padding */
159  float ___lon_float; /**< padding */
160  float altitude; /**< altitude in meters (AMSL) */
161  };
162  float params[7]; /**< array to store mission command values for MAV_FRAME_MISSION ***/
163  };
164 
165  uint16_t nav_cmd; /**< navigation command */
166 
167  int16_t do_jump_mission_index; /**< index where the do jump will go to */
168  uint16_t do_jump_repeat_count; /**< how many times do jump needs to be done */
169 
170  union {
171  uint16_t do_jump_current_count; /**< count how many times the jump has been done */
172  uint16_t vertex_count; /**< Polygon vertex count (geofence) */
173  uint16_t land_precision; /**< Defines if landing should be precise: 0 = normal landing, 1 = opportunistic precision landing, 2 = required precision landing (with search) */
174  };
175 
176  struct {
177  uint16_t frame : 4, /**< mission frame */
178  origin : 3, /**< how the mission item was generated */
179  loiter_exit_xtrack : 1, /**< exit xtrack location: 0 for center of loiter wp, 1 for exit location */
180  force_heading : 1, /**< heading needs to be reached */
181  altitude_is_relative : 1, /**< true if altitude is relative from start point */
182  autocontinue : 1, /**< true if next waypoint should follow after this one */
183  vtol_back_transition : 1, /**< part of the vtol back transition sequence */
184  _padding0 : 4; /**< padding remaining unused bits */
185  };
186 
187  uint8_t _padding1[2]; /**< padding struct size to alignment boundary */
188 };
189 
190 /**
191  * dataman housekeeping information for a specific item.
192  * Corresponds to the first dataman entry of DM_KEY_FENCE_POINTS and DM_KEY_SAFE_POINTS
193  */
195  uint16_t num_items; /**< total number of items stored (excluding this one) */
196  uint16_t update_counter; /**< This counter is increased when (some) items change (this can wrap) */
197 };
198 
199 /**
200  * Geofence vertex point.
201  * Corresponds to the DM_KEY_FENCE_POINTS dataman item
202  */
204  double lat;
205  double lon;
206  float alt;
207 
208  union {
209  uint16_t vertex_count; /**< number of vertices in this polygon */
210  float circle_radius; /**< geofence circle radius in meters (only used for NAV_CMD_NAV_FENCE_CIRCLE*) */
211  };
212 
213  uint16_t nav_cmd; /**< navigation command (one of MAV_CMD_NAV_FENCE_*) */
214  uint8_t frame; /**< MAV_FRAME */
215 
216  uint8_t _padding0[5]; /**< padding struct size to alignment boundary */
217 };
218 
219 /**
220  * Safe Point (Rally Point).
221  * Corresponds to the DM_KEY_SAFE_POINTS dataman item
222  */
224  double lat;
225  double lon;
226  float alt;
227  uint8_t frame; /**< MAV_FRAME */
228 
229  uint8_t _padding0[3]; /**< padding struct size to alignment boundary */
230 };
231 
232 #if (__GNUC__ >= 5) || __clang__
233 #pragma GCC diagnostic pop
234 #endif // GCC >= 5 || Clang
235 
236 #include <uORB/topics/mission.h>
237 
238 /**
239  * @}
240  */
Global position setpoint in WGS84 coordinates.
Definition: navigation.h:145
uint16_t frame
mission frame
Definition: navigation.h:177
uint16_t vertex_count
Polygon vertex count (geofence)
Definition: navigation.h:172
float altitude
altitude in meters (AMSL)
Definition: navigation.h:160
uint16_t autocontinue
true if next waypoint should follow after this one
Definition: navigation.h:177
float acceptance_radius
default radius in which the mission is accepted as reached in meters
Definition: navigation.h:155
uint16_t vtol_back_transition
part of the vtol back transition sequence
Definition: navigation.h:177
uint16_t origin
how the mission item was generated
Definition: navigation.h:177
uint16_t vertex_count
number of vertices in this polygon
Definition: navigation.h:209
uint16_t do_jump_repeat_count
how many times do jump needs to be done
Definition: navigation.h:168
float circle_radius
geofence circle radius in meters (only used for NAV_CMD_NAV_FENCE_CIRCLE*)
Definition: navigation.h:210
double lon
longitude in degrees
Definition: navigation.h:147
float ___lat_float
padding
Definition: navigation.h:158
float ___lon_float
padding
Definition: navigation.h:159
uint16_t _padding0
padding remaining unused bits
Definition: navigation.h:177
Geofence vertex point.
Definition: navigation.h:203
float circle_radius
geofence circle radius in meters (only used for NAV_CMD_NAV_FENCE_CIRCLE*)
Definition: navigation.h:153
uint16_t nav_cmd
navigation command
Definition: navigation.h:165
uint16_t do_jump_current_count
count how many times the jump has been done
Definition: navigation.h:171
dataman housekeeping information for a specific item.
Definition: navigation.h:194
float time_inside
time that the MAV should stay inside the radius before advancing in seconds
Definition: navigation.h:151
Safe Point (Rally Point).
Definition: navigation.h:223
float params[7]
array to store mission command values for MAV_FRAME_MISSION
Definition: navigation.h:162
uint16_t land_precision
Defines if landing should be precise: 0 = normal landing, 1 = opportunistic precision landing...
Definition: navigation.h:173
uint16_t nav_cmd
navigation command (one of MAV_CMD_NAV_FENCE_*)
Definition: navigation.h:213
uint16_t num_items
total number of items stored (excluding this one)
Definition: navigation.h:195
int16_t do_jump_mission_index
index where the do jump will go to
Definition: navigation.h:167
uint16_t update_counter
This counter is increased when (some) items change (this can wrap)
Definition: navigation.h:196
uint8_t frame
MAV_FRAME.
Definition: navigation.h:227
float pitch_min
minimal pitch angle for fixed wing takeoff waypoints
Definition: navigation.h:152
uint16_t loiter_exit_xtrack
exit xtrack location: 0 for center of loiter wp, 1 for exit location
Definition: navigation.h:177
uint8_t _padding1[2]
padding struct size to alignment boundary
Definition: navigation.h:187
uint16_t force_heading
heading needs to be reached
Definition: navigation.h:177
float yaw
in radians NED -PI..+PI, NAN means don&#39;t change yaw
Definition: navigation.h:157
float loiter_radius
loiter radius in meters, 0 for a VTOL to hover, negative for counter-clockwise
Definition: navigation.h:156
uint8_t frame
MAV_FRAME.
Definition: navigation.h:214
uint16_t altitude_is_relative
true if altitude is relative from start point
Definition: navigation.h:177
double lat
latitude in degrees
Definition: navigation.h:146