Line data Source code
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 : */
15 :
16 : #include "AP_Proximity_Boundary_3D.h"
17 :
18 : #define PROXIMITY_BOUNDARY_3D_TIMEOUT_MS 750 // we should check the 3D boundary faces after this many ms
19 :
20 : /*
21 : Constructor.
22 : This incorporates initialisation as well.
23 : */
24 62974 : AP_Proximity_Boundary_3D::AP_Proximity_Boundary_3D()
25 : {
26 : // initialise sector edge vector used for building the boundary fence
27 1369 : init();
28 1369 : }
29 :
30 : // initialise the boundary and sector_edge_vector array used for object avoidance
31 : // should be called if the sector_middle_deg or _sector_width_deg arrays are changed
32 1369 : void AP_Proximity_Boundary_3D::init()
33 : {
34 8214 : for (uint8_t layer=0; layer < PROXIMITY_NUM_LAYERS; layer++) {
35 6845 : const float pitch = ((float)_pitch_middle_deg[layer]);
36 61605 : for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
37 54760 : const float angle_rad = ((float)_sector_middle_deg[sector]+(PROXIMITY_SECTOR_WIDTH_DEG/2.0f));
38 54760 : _sector_edge_vector[layer][sector].offset_bearing(angle_rad, pitch, 100.0f);
39 54760 : _boundary_points[layer][sector] = _sector_edge_vector[layer][sector] * PROXIMITY_BOUNDARY_DIST_DEFAULT;
40 : }
41 : }
42 1369 : }
43 :
44 : // returns face corresponding to the provided yaw and (optionally) pitch
45 : // pitch is the vertical body-frame angle (in degrees) to the obstacle (0=directly ahead, 90 is above the vehicle)
46 : // yaw is the horizontal body-frame angle (in degrees) to the obstacle (0=directly ahead of the vehicle, 90 is to the right of the vehicle)
47 407369 : AP_Proximity_Boundary_3D::Face AP_Proximity_Boundary_3D::get_face(float pitch, float yaw) const
48 : {
49 407369 : const uint8_t sector = wrap_360(yaw + (PROXIMITY_SECTOR_WIDTH_DEG * 0.5f)) / 45.0f;
50 407369 : const float pitch_limited = constrain_float(pitch, -75.0f, 74.9f);
51 407369 : const uint8_t layer = (pitch_limited + 75.0f)/PROXIMITY_PITCH_WIDTH_DEG;
52 407369 : return Face{layer, sector};
53 : }
54 :
55 : // Set the actual body-frame angle(yaw), pitch, and distance of the detected object.
56 : // This method will also mark the sector and layer to be "valid",
57 : // This distance can then be used for Obstacle Avoidance
58 : // Assume detected obstacle is horizontal (zero pitch), if no pitch is passed
59 : // prx_instance should be set to the proximity sensor backend instance number
60 314943 : void AP_Proximity_Boundary_3D::set_face_attributes(const Face &face, float pitch, float angle, float distance, uint8_t prx_instance)
61 : {
62 314943 : if (!face.valid()) {
63 0 : return;
64 : }
65 :
66 : // ignore update if another instance has provided a shorter distance within the last 0.2 seconds
67 314943 : if ((prx_instance != _prx_instance[face.layer][face.sector]) && _distance_valid[face.layer][face.sector] && (_filtered_distance[face.layer][face.sector].get() < distance)) {
68 : // check if recent
69 0 : const uint32_t now_ms = AP_HAL::millis();
70 0 : if (now_ms - _last_update_ms[face.layer][face.sector] < PROXIMITY_FACE_RESET_MS) {
71 0 : return;
72 : }
73 : }
74 :
75 314943 : _angle_deg[face.layer][face.sector] = angle;
76 314943 : _pitch_deg[face.layer][face.sector] = pitch;
77 314943 : _distance[face.layer][face.sector] = distance;
78 314943 : _distance_valid[face.layer][face.sector] = true;
79 314943 : _prx_instance[face.layer][face.sector] = prx_instance;
80 :
81 : // apply filter
82 314943 : set_filtered_distance(face, distance);
83 :
84 : // update boundary used for simple avoidance
85 314943 : update_boundary(face);
86 : }
87 :
88 : // apply a new cutoff_freq to low-pass filter
89 12 : void AP_Proximity_Boundary_3D::apply_filter_freq(float cutoff_freq)
90 : {
91 72 : for (uint8_t layer=0; layer < PROXIMITY_NUM_LAYERS; layer++) {
92 540 : for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
93 480 : _filtered_distance[layer][sector].set_cutoff_frequency(cutoff_freq);
94 : }
95 : }
96 12 : }
97 :
98 : // Apply low pass filter on the raw distance
99 314943 : void AP_Proximity_Boundary_3D::set_filtered_distance(const Face &face, float distance)
100 : {
101 314943 : if (!face.valid()) {
102 0 : return;
103 : }
104 314943 : if (!is_equal(_filtered_distance[face.layer][face.sector].get_cutoff_freq(), _filter_freq)) {
105 : // cutoff freq has changed
106 12 : apply_filter_freq(_filter_freq);
107 : }
108 :
109 314943 : const uint32_t now_ms = AP_HAL::millis();
110 314943 : const uint32_t dt = now_ms - _last_update_ms[face.layer][face.sector];
111 314943 : if (dt < PROXIMITY_FILT_RESET_TIME) {
112 314761 : _filtered_distance[face.layer][face.sector].apply(distance, dt* 0.001f);
113 : } else {
114 : // reset filter since last distance was passed a long time back
115 182 : _filtered_distance[face.layer][face.sector].reset(distance);
116 : }
117 314943 : _last_update_ms[face.layer][face.sector] = now_ms;
118 : }
119 :
120 : // update boundary points used for object avoidance based on a single sector and pitch distance changing
121 : // the boundary points lie on the line between sectors meaning two boundary points may be updated based on a single sector's distance changing
122 : // the boundary point is set to the shortest distance found in the two adjacent sectors, this is a conservative boundary around the vehicle
123 315010 : void AP_Proximity_Boundary_3D::update_boundary(const Face &face)
124 : {
125 : // sanity check
126 315010 : if (!face.valid()) {
127 0 : return;
128 : }
129 :
130 315010 : const uint8_t layer = face.layer;
131 315010 : const uint8_t sector = face.sector;
132 :
133 : // find adjacent sector (clockwise)
134 315010 : const uint8_t next_sector = get_next_sector(sector);
135 :
136 : // boundary point lies on the line between the two sectors at the shorter distance found in the two sectors
137 315010 : float shortest_distance = PROXIMITY_BOUNDARY_DIST_DEFAULT;
138 315010 : if (_distance_valid[layer][sector] && _distance_valid[layer][next_sector]) {
139 314271 : shortest_distance = MIN(_filtered_distance[layer][sector].get(), _filtered_distance[layer][next_sector].get());
140 739 : } else if (_distance_valid[layer][sector]) {
141 672 : shortest_distance = _filtered_distance[layer][sector].get();
142 67 : } else if (_distance_valid[layer][next_sector]) {
143 56 : shortest_distance = _filtered_distance[layer][next_sector].get();
144 : }
145 315010 : if (shortest_distance < PROXIMITY_BOUNDARY_DIST_MIN) {
146 9266 : shortest_distance = PROXIMITY_BOUNDARY_DIST_MIN;
147 : }
148 315010 : _boundary_points[layer][sector] = _sector_edge_vector[layer][sector] * shortest_distance;
149 :
150 : // if the next sector (clockwise) has an invalid distance, set boundary to create a cup like boundary
151 315010 : if (!_distance_valid[layer][next_sector]) {
152 683 : _boundary_points[layer][next_sector] = _sector_edge_vector[layer][next_sector] * shortest_distance;
153 : }
154 :
155 : // repeat for edge between sector and previous sector
156 315010 : const uint8_t prev_sector = get_prev_sector(sector);
157 315010 : shortest_distance = PROXIMITY_BOUNDARY_DIST_DEFAULT;
158 315010 : if (_distance_valid[layer][prev_sector] && _distance_valid[layer][sector]) {
159 314376 : shortest_distance = MIN(_filtered_distance[layer][prev_sector].get(), _filtered_distance[layer][sector].get());
160 634 : } else if (_distance_valid[layer][prev_sector]) {
161 30 : shortest_distance = _filtered_distance[layer][prev_sector].get();
162 604 : } else if (_distance_valid[layer][sector]) {
163 567 : shortest_distance = _filtered_distance[layer][sector].get();
164 : }
165 315010 : _boundary_points[layer][prev_sector] = _sector_edge_vector[layer][prev_sector] * shortest_distance;
166 :
167 : // if the sector counter-clockwise from the previous sector has an invalid distance, set boundary to create a cup-like boundary
168 315010 : const uint8_t prev_sector_ccw = get_prev_sector(prev_sector);
169 315010 : if (!_distance_valid[layer][prev_sector_ccw]) {
170 653 : _boundary_points[layer][prev_sector_ccw] = _sector_edge_vector[layer][prev_sector_ccw] * shortest_distance;
171 : }
172 : }
173 :
174 : // reset boundary. marks all distances as invalid
175 3 : void AP_Proximity_Boundary_3D::reset()
176 : {
177 18 : for (uint8_t layer=0; layer < PROXIMITY_NUM_LAYERS; layer++) {
178 135 : for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
179 120 : _distance_valid[layer][sector] = false;
180 : }
181 : }
182 3 : }
183 :
184 : // Reset this location, specified by Face object, back to default
185 : // i.e Distance is marked as not-valid, and set to a large number.
186 : // prx_instance should be set to the proximity sensor's backend instance number
187 3724 : void AP_Proximity_Boundary_3D::reset_face(const Face &face, uint8_t prx_instance)
188 : {
189 3724 : if (!face.valid()) {
190 4 : return;
191 : }
192 :
193 : // return immediately if face already has no valid distance
194 3720 : if (!_distance_valid[face.layer][face.sector]) {
195 3713 : return;
196 : }
197 :
198 : // ignore reset if another instance provided this face's distance within the last 0.2 seconds
199 7 : if (prx_instance != _prx_instance[face.layer][face.sector]) {
200 0 : const uint32_t now_ms = AP_HAL::millis();
201 0 : if (now_ms - _last_update_ms[face.layer][face.sector] < 200) {
202 0 : return;
203 : }
204 : }
205 :
206 7 : _distance_valid[face.layer][face.sector] = false;
207 :
208 : // update simple avoidance boundary
209 7 : update_boundary(face);
210 : }
211 :
212 : // check if a face has valid distance even if it was updated a long time back
213 15835009 : void AP_Proximity_Boundary_3D::check_face_timeout()
214 : {
215 : // exit immediately if already checked recently
216 15835009 : const uint32_t now_ms = AP_HAL::millis();
217 15835009 : if ((now_ms - _last_check_face_timeout_ms) < PROXIMITY_BOUNDARY_3D_TIMEOUT_MS) {
218 15716469 : return;
219 : }
220 118540 : _last_check_face_timeout_ms = now_ms;
221 :
222 711240 : for (uint8_t layer=0; layer < PROXIMITY_NUM_LAYERS; layer++) {
223 5334300 : for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
224 4741600 : if (_distance_valid[layer][sector]) {
225 4357 : if ((now_ms - _last_update_ms[layer][sector]) > PROXIMITY_FACE_RESET_MS) {
226 : // this face has a valid distance but wasn't updated for a long time, reset it
227 60 : _distance_valid[layer][sector] = false;
228 60 : update_boundary(AP_Proximity_Boundary_3D::Face{layer, sector});
229 : }
230 : }
231 : }
232 : }
233 : }
234 :
235 : // get distance for a face. returns true on success and fills in distance argument with distance in meters
236 348192 : bool AP_Proximity_Boundary_3D::get_distance(const Face &face, float &distance) const
237 : {
238 348192 : if (!face.valid()) {
239 0 : return false;
240 : }
241 348192 : if (_distance_valid[face.layer][face.sector]) {
242 52119 : distance = _distance[face.layer][face.sector];
243 52119 : return true;
244 : }
245 :
246 296073 : return false;
247 : }
248 :
249 : // get the total number of obstacles
250 2006560 : uint8_t AP_Proximity_Boundary_3D::get_obstacle_count() const
251 : {
252 2006560 : return PROXIMITY_NUM_LAYERS * PROXIMITY_NUM_SECTORS;
253 : }
254 :
255 : // Converts obstacle_num passed from avoidance library into appropriate face of the boundary
256 : // Returns false if the face is invalid
257 : // "update_boundary" method manipulates two sectors ccw and one sector cw from any valid face.
258 : // Any boundary that does not fall into these manipulated faces are useless, and will be marked as false
259 : // The resultant is packed into a Boundary Location object and returned by reference as "face"
260 80263576 : bool AP_Proximity_Boundary_3D::convert_obstacle_num_to_face(uint8_t obstacle_num, Face& face) const
261 : {
262 : // obstacle num is just "flattened layers, and sectors"
263 80263576 : const uint8_t layer = obstacle_num / PROXIMITY_NUM_SECTORS;
264 80263576 : const uint8_t sector = obstacle_num % PROXIMITY_NUM_SECTORS;
265 80263576 : face.sector = sector;
266 80263576 : face.layer = layer;
267 :
268 80263576 : uint8_t valid_sector = sector;
269 : // check for 3 adjacent sectors
270 320076598 : for (uint8_t i=0; i < 3; i++) {
271 240140243 : if (_distance_valid[layer][valid_sector]) {
272 : // update boundary has manipulated this face
273 327221 : return true;
274 : }
275 239813022 : valid_sector = get_next_sector(valid_sector);
276 : }
277 :
278 : // this face was not manipulated by "update_boundary" and is stale. Don't use it
279 79936355 : return false;
280 : }
281 :
282 : // Appropriate layer and sector are found from the passed obstacle_num
283 : // This function then draws a line between this sector, and sector + 1 at the given layer
284 : // Then returns the closest point on this line from vehicle, in body-frame.
285 : // Used by GPS based Simple Avoidance
286 : // False is returned if the obstacle_num provided does not produce a valid obstacle
287 80262400 : bool AP_Proximity_Boundary_3D::get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const
288 : {
289 80262400 : Face face;
290 80262400 : if (!convert_obstacle_num_to_face(obstacle_num, face)) {
291 : // not a valid face
292 79936355 : return false;
293 : }
294 326045 : const uint8_t sector_end = face.sector;
295 326045 : const uint8_t sector_start = get_next_sector(face.sector);
296 :
297 326045 : const Vector3f start = _boundary_points[face.layer][sector_start];
298 326045 : const Vector3f end = _boundary_points[face.layer][sector_end];
299 326045 : vec_to_obstacle = Vector3f::point_on_line_closest_to_other_point(start, end, Vector3f{});
300 326045 : return true;
301 : }
302 :
303 : // Appropriate layer and sector are found from the passed obstacle_num
304 : // This function then draws a line between this sector, and sector + 1 at the given layer
305 : // Then returns the closest point on this line from the segment that was passed, in body-frame.
306 : // Addionally a 3-D plane is constructed using the closest point found above as normal, and a point on the line segment in the boundary.
307 : // True is returned when the passed line segment intersects this plane.
308 : // This helps us know if the passed line segment was in the direction of the boundary, or going in a different direction.
309 : // Used by GPS based Simple Avoidance - for "brake mode"
310 : // False is returned if the obstacle_num provided does not produce a valid obstacle
311 1176 : bool AP_Proximity_Boundary_3D::closest_point_from_segment_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const
312 : {
313 1176 : Face face;
314 1176 : if (!convert_obstacle_num_to_face(obstacle_num, face)) {
315 : // not a valid a face
316 0 : return false;
317 : }
318 :
319 1176 : const uint8_t sector_end = face.sector;
320 1176 : const uint8_t sector_start = get_next_sector(face.sector);
321 1176 : const Vector3f start = _boundary_points[face.layer][sector_start];
322 1176 : const Vector3f end = _boundary_points[face.layer][sector_end];
323 :
324 : // closest point between passed line segment and boundary
325 1176 : Vector3f::segment_to_segment_closest_point(seg_start, seg_end, start, end, closest_point);
326 1176 : if (closest_point == start) {
327 : // draw a plane using the closest point as normal vector, and a point on the boundary
328 : // return false if the passed segment does not intersect the plane
329 166 : return Vector3f::segment_plane_intersect(seg_start, seg_end, closest_point, end);
330 : }
331 1010 : return Vector3f::segment_plane_intersect(seg_start, seg_end, closest_point, start);
332 : }
333 :
334 : // get distance and angle to closest object (used for pre-arm check)
335 : // returns true on success, false if no valid readings
336 93666 : bool AP_Proximity_Boundary_3D::get_closest_object(float& angle_deg, float &distance) const
337 : {
338 93666 : bool closest_found = false;
339 93666 : uint8_t closest_sector = 0;
340 93666 : uint8_t closest_layer = 0;
341 :
342 : // check boundary for shortest distance
343 : // only check for middle layers and higher
344 : // lower layers might contain ground, which will give false pre-arm failure
345 374664 : for (uint8_t layer=PROXIMITY_MIDDLE_LAYER; layer<PROXIMITY_NUM_LAYERS; layer++) {
346 2528982 : for (uint8_t sector=0; sector<PROXIMITY_NUM_SECTORS; sector++) {
347 2247984 : if (_distance_valid[layer][sector]) {
348 34107 : if (!closest_found || (_distance[layer][sector] < _distance[closest_layer][closest_sector])) {
349 11840 : closest_layer = layer;
350 11840 : closest_sector = sector;
351 11840 : closest_found = true;
352 : }
353 : }
354 : }
355 : }
356 :
357 93666 : if (closest_found) {
358 5426 : angle_deg = _angle_deg[closest_layer][closest_sector];
359 5426 : distance = _distance[closest_layer][closest_sector];
360 : }
361 93666 : return closest_found;
362 : }
363 :
364 : // get number of objects, used for non-GPS avoidance
365 0 : uint8_t AP_Proximity_Boundary_3D::get_horizontal_object_count() const
366 : {
367 0 : return PROXIMITY_NUM_SECTORS;
368 : }
369 :
370 : // get an object's angle and distance, used for non-GPS avoidance
371 : // returns false if no angle or distance could be returned for some reason
372 0 : bool AP_Proximity_Boundary_3D::get_horizontal_object_angle_and_distance(uint8_t object_number, float &angle_deg, float &distance) const
373 : {
374 0 : if ((object_number < PROXIMITY_NUM_SECTORS) && _distance_valid[PROXIMITY_MIDDLE_LAYER][object_number]) {
375 0 : angle_deg = _angle_deg[PROXIMITY_MIDDLE_LAYER][object_number];
376 0 : distance = _filtered_distance[PROXIMITY_MIDDLE_LAYER][object_number].get();
377 0 : return true;
378 : }
379 0 : return false;
380 : }
381 :
382 : // get an obstacle info for AP_Periph
383 : // returns false if no angle or distance could be returned for some reason
384 0 : bool AP_Proximity_Boundary_3D::get_obstacle_info(uint8_t obstacle_num, float &angle_deg, float &pitch_deg, float &distance) const
385 : {
386 : // obstacle num is just "flattened layers, and sectors"
387 0 : const uint8_t layer = obstacle_num / PROXIMITY_NUM_SECTORS;
388 0 : const uint8_t sector = obstacle_num % PROXIMITY_NUM_SECTORS;
389 0 : if (_distance_valid[layer][sector]) {
390 0 : angle_deg = _angle_deg[layer][sector];
391 0 : pitch_deg = _pitch_deg[layer][sector];
392 0 : distance = _filtered_distance[layer][sector].get();
393 0 : return true;
394 : }
395 :
396 0 : return false;
397 : }
398 :
399 : // Return filtered distance for the passed in face
400 52119 : bool AP_Proximity_Boundary_3D::get_filtered_distance(const Face &face, float &distance) const
401 : {
402 52119 : if (!face.valid()) {
403 0 : return false;
404 : }
405 :
406 52119 : if (!_distance_valid[face.layer][face.sector]) {
407 : // invalid distace
408 0 : return false;
409 : }
410 :
411 52119 : distance = _filtered_distance[face.layer][face.sector].get();
412 52119 : return true;
413 : }
414 :
415 : // Get raw and filtered distances in 8 directions per layer
416 43524 : bool AP_Proximity_Boundary_3D::get_layer_distances(uint8_t layer_number, float dist_max, Proximity_Distance_Array &prx_dist_array, Proximity_Distance_Array &prx_filt_dist_array) const
417 : {
418 : // cycle through all sectors filling in distances and orientations
419 : // see MAV_SENSOR_ORIENTATION for orientations (0 = forward, 1 = 45 degree clockwise from north, etc)
420 43524 : bool valid_distances = false;
421 43524 : prx_dist_array.offset_valid = 0;
422 43524 : prx_filt_dist_array.offset_valid = 0;
423 391716 : for (uint8_t i=0; i<PROXIMITY_MAX_DIRECTION; i++) {
424 348192 : prx_dist_array.orientation[i] = i;
425 348192 : const AP_Proximity_Boundary_3D::Face face(layer_number, i);
426 348192 : if (!face.valid()) {
427 0 : return false;
428 : }
429 348192 : if (get_distance(face, prx_dist_array.distance[i]) && get_filtered_distance(face, prx_filt_dist_array.distance[i])) {
430 52119 : valid_distances = true;
431 52119 : prx_dist_array.offset_valid |= (1U << i);
432 52119 : prx_filt_dist_array.offset_valid |= (1U << i);
433 : } else {
434 296073 : prx_dist_array.distance[i] = dist_max;
435 296073 : prx_filt_dist_array.distance[i] = dist_max;
436 : }
437 : }
438 :
439 43524 : return valid_distances;
440 : }
441 :
442 : // reset the temporary boundary. This fills in distances with FLT_MAX
443 447 : void AP_Proximity_Temp_Boundary::reset()
444 : {
445 2682 : for (uint8_t layer=0; layer < PROXIMITY_NUM_LAYERS; layer++) {
446 20115 : for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
447 17880 : _distances[layer][sector] = FLT_MAX;
448 : }
449 : }
450 447 : }
451 :
452 : // add a distance to the temp boundary if it is shorter than any other provided distance since the last time the boundary was reset
453 : // pitch and yaw are in degrees, distance is in meters
454 457 : void AP_Proximity_Temp_Boundary::add_distance(const AP_Proximity_Boundary_3D::Face &face, float pitch_deg, float yaw_deg, float distance_m)
455 : {
456 457 : if (face.valid() && distance_m < _distances[face.layer][face.sector]) {
457 457 : _distances[face.layer][face.sector] = distance_m;
458 457 : _angle_deg[face.layer][face.sector] = yaw_deg;
459 457 : _pitch_deg[face.layer][face.sector] = pitch_deg;
460 : }
461 457 : }
462 :
463 : // fill the original 3D boundary with the contents of this temporary boundary
464 : // prx_instance should be set to the proximity sensor's backend instance number
465 443 : void AP_Proximity_Temp_Boundary::update_3D_boundary(uint8_t prx_instance, AP_Proximity_Boundary_3D &boundary)
466 : {
467 2658 : for (uint8_t layer=0; layer < PROXIMITY_NUM_LAYERS; layer++) {
468 19935 : for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
469 17720 : if (_distances[layer][sector] < FLT_MAX) {
470 447 : AP_Proximity_Boundary_3D::Face face{layer, sector};
471 447 : boundary.set_face_attributes(face, _pitch_deg[layer][sector], _angle_deg[layer][sector], _distances[layer][sector], prx_instance);
472 : }
473 : }
474 : }
475 443 : }
476 :
|
