forked from ArduPilot/ardupilot
/
AP_Proximity.h
249 lines (193 loc) · 7.98 KB
/
AP_Proximity.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "AP_Proximity_config.h"
#if HAL_PROXIMITY_ENABLED
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include "AP_Proximity_Params.h"
#include "AP_Proximity_Boundary_3D.h"
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_HAL/Semaphores.h>
#define PROXIMITY_MAX_INSTANCES 3 // Maximum number of proximity sensor instances available on this platform
#define PROXIMITY_SENSOR_ID_START 10
class AP_Proximity_Backend;
class AP_Proximity
{
public:
friend class AP_Proximity_Backend;
friend class AP_Proximity_DroneCAN;
AP_Proximity();
/* Do not allow copies */
CLASS_NO_COPY(AP_Proximity);
// Proximity driver types
enum class Type {
None = 0,
// 1 was SF40C_v09
#if AP_PROXIMITY_MAV_ENABLED
MAV = 2,
#endif
#if AP_PROXIMITY_TERARANGERTOWER_ENABLED
TRTOWER = 3,
#endif
#if AP_PROXIMITY_RANGEFINDER_ENABLED
RangeFinder = 4,
#endif
#if AP_PROXIMITY_RPLIDARA2_ENABLED
RPLidarA2 = 5,
#endif
#if AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED
TRTOWEREVO = 6,
#endif
#if AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED
SF40C = 7,
#endif
#if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED
SF45B = 8,
#endif
#if AP_PROXIMITY_SITL_ENABLED
SITL = 10,
#endif
#if AP_PROXIMITY_AIRSIMSITL_ENABLED
AirSimSITL = 12,
#endif
#if AP_PROXIMITY_CYGBOT_ENABLED
CYGBOT_D1 = 13,
#endif
#if AP_PROXIMITY_DRONECAN_ENABLED
DroneCAN = 14,
#endif
#if AP_PROXIMITY_SCRIPTING_ENABLED
Scripting = 15,
#endif
#if AP_PROXIMITY_LD06_ENABLED
LD06 = 16,
#endif
#if AP_PROXIMITY_MR72_ENABLED
MR72 = 17,
#endif
};
enum class Status {
NotConnected = 0,
NoData,
Good
};
// detect and initialise any available proximity sensors
void init();
// update state of all proximity sensors. Should be called at high rate from main loop
void update();
// return the number of proximity sensor backends
uint8_t num_sensors() const { return num_instances; }
// return sensor type of a given instance
Type get_type(uint8_t instance) const;
// return distance filter frequency
float get_filter_freq() const { return _filt_freq; }
// return sensor health
Status get_instance_status(uint8_t instance) const;
// Returns status of first good sensor. If no good sensor found, returns status of last instance sensor
Status get_status() const;
// prearm checks
bool prearm_healthy(char *failure_msg, const uint8_t failure_msg_len) const;
// get maximum and minimum distances (in meters)
float distance_max() const;
float distance_min() const;
//
// 3D boundary related methods
//
// get distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
bool get_horizontal_distances(Proximity_Distance_Array &prx_dist_array) const;
// get total number of obstacles, used in GPS based Simple Avoidance
uint8_t get_obstacle_count() const;
// get vector to obstacle based on obstacle_num passed, used in GPS based Simple Avoidance
bool get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const;
// returns shortest distance to "obstacle_num" obstacle, from a line segment formed between "seg_start" and "seg_end"
// returns FLT_MAX if it's an invalid instance.
bool closest_point_from_segment_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const;
// get distance and angle to closest object (used for pre-arm check)
// returns true on success, false if no valid readings
bool get_closest_object(float& angle_deg, float &distance) const;
// get number of objects
uint8_t get_object_count() const;
bool get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const;
// get obstacle pitch and angle for a particular obstacle num
bool get_obstacle_info(uint8_t obstacle_num, float &angle_deg, float &pitch, float &distance) const;
//
// mavlink related methods
//
// handle mavlink messages
void handle_msg(const mavlink_message_t &msg);
// methods for mavlink SYS_STATUS message (send_sys_status)
bool sensor_present() const;
bool sensor_enabled() const;
bool sensor_failed() const;
//
// support for upwards and downwards facing sensors
//
// get distance upwards in meters. returns true on success
bool get_upward_distance(uint8_t instance, float &distance) const;
bool get_upward_distance(float &distance) const;
// set alt as read from downward facing rangefinder. Tilt is already adjusted for
void set_rangefinder_alt(bool use, bool healthy, float alt_cm);
// method called by vehicle to have AP_Proximity write onboard log messages
void log();
// The Proximity_State structure is filled in by the backend driver
struct Proximity_State {
uint8_t instance; // the instance number of this proximity sensor
Status status; // sensor status
const struct AP_Param::GroupInfo *var_info; // stores extra parameter information for the sensor (if it exists)
};
static const struct AP_Param::GroupInfo *backend_var_info[PROXIMITY_MAX_INSTANCES];
// parameter list
static const struct AP_Param::GroupInfo var_info[];
static AP_Proximity *get_singleton(void) { return _singleton; };
// return backend object for Lua scripting
AP_Proximity_Backend *get_backend(uint8_t id) const;
// 3D boundary
AP_Proximity_Boundary_3D boundary;
// Check if Obstacle defined by body-frame yaw and pitch is near ground
bool check_obstacle_near_ground(float pitch, float yaw, float distance) const;
// get proximity address (for AP_Periph CAN)
uint8_t get_address(uint8_t id) const {
return id >= PROXIMITY_MAX_INSTANCES? 0 : uint8_t(params[id].address.get());
}
protected:
// parameters for backends
AP_Proximity_Params params[PROXIMITY_MAX_INSTANCES];
private:
static AP_Proximity *_singleton;
Proximity_State state[PROXIMITY_MAX_INSTANCES];
AP_Proximity_Backend *drivers[PROXIMITY_MAX_INSTANCES];
uint8_t num_instances;
// return true if the given instance exists
bool valid_instance(uint8_t i) const;
// parameters for all instances
AP_Int8 _raw_log_enable; // enable logging raw distances
AP_Int8 _ign_gnd_enable; // true if land detection should be enabled
AP_Float _filt_freq; // cutoff frequency for low pass filter
AP_Float _alt_min; // Minimum altitude -in meters- below which proximity should not work.
// get alt from rangefinder in meters. This reading is corrected for vehicle tilt
bool get_rangefinder_alt(float &alt_m) const;
struct RangeFinderState {
bool use; // true if enabled
bool healthy; // true if we can trust the altitude from the rangefinder
int16_t alt_cm; // tilt compensated altitude (in cm) from rangefinder
uint32_t last_downward_update_ms; // last update ms
} _rangefinder_state;
HAL_Semaphore detect_sem;
};
namespace AP {
AP_Proximity *proximity();
};
#endif // HAL_PROXIMITY_ENABLED