APM:Libraries
AP_Common.h
Go to the documentation of this file.
1 /*
2  This program is free software: you can redistribute it and/or modify
3  it under the terms of the GNU General Public License as published by
4  the Free Software Foundation, either version 3 of the License, or
5  (at your option) any later version.
6 
7  This program is distributed in the hope that it will be useful,
8  but WITHOUT ANY WARRANTY; without even the implied warranty of
9  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10  GNU General Public License for more details.
11 
12  You should have received a copy of the GNU General Public License
13  along with this program. If not, see <http://www.gnu.org/licenses/>.
14  */
20 
21 #pragma once
22 
23 #include <AP_HAL/AP_HAL_Boards.h>
24 #include <stdint.h>
25 #include <stdlib.h>
26 
27 // used to pack structures
28 #define PACKED __attribute__((__packed__))
29 
30 // used to mark a function that may be unused in some builds
31 #define UNUSED_FUNCTION __attribute__((unused))
32 
33 // this can be used to optimize individual functions
34 #define OPTIMIZE(level) __attribute__((optimize(level)))
35 
36 // sometimes we need to prevent inlining to prevent large stack usage
37 #define NOINLINE __attribute__((noinline))
38 
39 #define FMT_PRINTF(a,b) __attribute__((format(printf, a, b)))
40 #define FMT_SCANF(a,b) __attribute__((format(scanf, a, b)))
41 
42 #ifdef __has_cpp_attribute
43 # if __has_cpp_attribute(fallthrough)
44 # define FALLTHROUGH [[fallthrough]]
45 # elif __has_cpp_attribute(gnu::fallthrough)
46 # define FALLTHROUGH [[gnu::fallthrough]]
47 # endif
48 #endif
49 #ifndef FALLTHROUGH
50 # define FALLTHROUGH
51 #endif
52 
53 #define ToRad(x) radians(x) // *pi/180
54 #define ToDeg(x) degrees(x) // *180/pi
55 
56 /* Declare and implement const and non-const versions of the array subscript
57  * operator. The object is treated as an array of type_ values. */
58 #define DEFINE_BYTE_ARRAY_METHODS \
59  inline uint8_t &operator[](size_t i) { return reinterpret_cast<uint8_t *>(this)[i]; } \
60  inline uint8_t operator[](size_t i) const { return reinterpret_cast<const uint8_t *>(this)[i]; }
61 
62 #define LOCATION_ALT_MAX_M 83000 // maximum altitude (in meters) that can be fit into Location structure's alt field
63 
64 /*
65  check if bit bitnumber is set in value, returned as a
66  bool. Bitnumber starts at 0 for the first bit
67  */
68 #define BIT_IS_SET(value, bitnumber) (((value) & (1U<<(bitnumber))) != 0)
69 
70 // get high or low bytes from 2 byte integer
71 #define LOWBYTE(i) ((uint8_t)(i))
72 #define HIGHBYTE(i) ((uint8_t)(((uint16_t)(i))>>8))
73 
74 template <typename T, size_t N>
75 char (&_ARRAY_SIZE_HELPER(T (&_arr)[N]))[N];
76 
77 template <typename T>
78 char (&_ARRAY_SIZE_HELPER(T (&_arr)[0]))[0];
79 
80 #define ARRAY_SIZE(_arr) sizeof(_ARRAY_SIZE_HELPER(_arr))
81 
82 // simpler ARRAY_SIZE which can handle zero elements
83 #define ARRAY_SIZE_SIMPLE(_arr) (sizeof(_arr)/sizeof(_arr[0]))
84 
85 /*
86  * See UNUSED_RESULT. The difference is that it receives @uniq_ as the name to
87  * be used for its internal variable.
88  *
89  * @uniq_: a unique name to use for variable name
90  * @expr_: the expression to be evaluated
91  */
92 #define _UNUSED_RESULT(uniq_, expr_) \
93  do { \
94  decltype(expr_) uniq_ __attribute__((unused)); \
95  uniq_ = expr_; \
96  } while (0)
97 
98 /*
99  * Allow to call a function annotated with warn_unused_result attribute
100  * without getting a warning, because sometimes this is what we want to do.
101  *
102  * @expr_: the expression to be evaluated
103  */
104 #define UNUSED_RESULT(expr_) _UNUSED_RESULT(__unique_name_##__COUNTER__, expr_)
105 
106 // @}
107 
108 
121 
123 
125  uint8_t relative_alt : 1; // 1 if altitude is relative to home
126  uint8_t unused1 : 1; // unused flag (defined so that loiter_ccw uses the correct bit)
127  uint8_t loiter_ccw : 1; // 0 if clockwise, 1 if counter clockwise
128  uint8_t terrain_alt : 1; // this altitude is above terrain
129  uint8_t origin_alt : 1; // this altitude is above ekf origin
130  uint8_t loiter_xtrack : 1; // 0 to crosstrack from center of waypoint, 1 to crosstrack from tangent exit location
131 };
132 
133 struct PACKED Location {
134  union {
136  uint8_t options;
137  };
138  // by making alt 24 bit we can make p1 in a command 16 bit,
139  // allowing an accurate angle in centi-degrees. This keeps the
140  // storage cost per mission item at 15 bytes, and allows mission
141  // altitudes of up to +/- 83km
142  int32_t alt:24;
143  int32_t lat;
144  int32_t lng;
145 };
146 
148 
154 
155 
156 /*
157  Return true if value is between lower and upper bound inclusive.
158  False otherwise.
159 */
160 bool is_bounded_int32(int32_t value, int32_t lower_bound, int32_t upper_bound);
161 
162 /*
163  useful debugging macro for SITL
164  */
165 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
166 #include <stdio.h>
167 #define SITL_printf(fmt, args ...) do { ::printf("%s(%u): " fmt, __FILE__, __LINE__, ##args); } while(0)
168 #else
169 #define SITL_printf(fmt, args ...)
170 #endif
char(& _ARRAY_SIZE_HELPER(T(&_arr)[N]))[N]
int32_t lat
param 3 - Latitude * 10**7
Definition: AP_Common.h:143
int32_t alt
param 2 - Altitude in centimeters (meters * 100) see LOCATION_ALT_MAX_M
Definition: AP_Common.h:142
bool is_bounded_int32(int32_t value, int32_t lower_bound, int32_t upper_bound)
Definition: AP_Common.cpp:29
Location_Option_Flags flags
options bitmask (1<<0 = relative altitude)
Definition: AP_Common.h:135
int32_t lng
param 4 - Longitude * 10**7
Definition: AP_Common.h:144
#define N
#define PACKED
Definition: AP_Common.h:28
float value
uint8_t options
Definition: AP_Common.h:136