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 : This program is distributed in the hope that it will be useful,
7 : but WITHOUT ANY WARRANTY; without even the implied warranty of
8 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
9 : GNU General Public License for more details.
10 : You should have received a copy of the GNU General Public License
11 : along with this program. If not, see <http://www.gnu.org/licenses/>.
12 : */
13 :
14 : #include "AC_Avoidance_config.h"
15 :
16 : #if AP_OADATABASE_ENABLED
17 :
18 : #include "AP_OADatabase.h"
19 :
20 : #include <AP_AHRS/AP_AHRS.h>
21 : #include <GCS_MAVLink/GCS.h>
22 : #include <AP_Math/AP_Math.h>
23 : #include <AP_Vehicle/AP_Vehicle_Type.h>
24 :
25 : extern const AP_HAL::HAL& hal;
26 :
27 : #ifndef AP_OADATABASE_TIMEOUT_SECONDS_DEFAULT
28 : #define AP_OADATABASE_TIMEOUT_SECONDS_DEFAULT 10
29 : #endif
30 :
31 : #ifndef AP_OADATABASE_SIZE_DEFAULT
32 : #define AP_OADATABASE_SIZE_DEFAULT 100
33 : #endif
34 :
35 : #ifndef AP_OADATABASE_QUEUE_SIZE_DEFAULT
36 : #define AP_OADATABASE_QUEUE_SIZE_DEFAULT 80
37 : #endif
38 :
39 : #ifndef AP_OADATABASE_DISTANCE_FROM_HOME
40 : #define AP_OADATABASE_DISTANCE_FROM_HOME 3
41 : #endif
42 :
43 : const AP_Param::GroupInfo AP_OADatabase::var_info[] = {
44 :
45 : // @Param: SIZE
46 : // @DisplayName: OADatabase maximum number of points
47 : // @Description: OADatabase maximum number of points. Set to 0 to disable the OA Database. Larger means more points but is more cpu intensive to process
48 : // @Range: 0 10000
49 : // @User: Advanced
50 : // @RebootRequired: True
51 : AP_GROUPINFO("SIZE", 1, AP_OADatabase, _database_size_param, AP_OADATABASE_SIZE_DEFAULT),
52 :
53 : // @Param: EXPIRE
54 : // @DisplayName: OADatabase item timeout
55 : // @Description: OADatabase item timeout. The time an item will linger without any updates before it expires. Zero means never expires which is useful for a sent-once static environment but terrible for dynamic ones.
56 : // @Units: s
57 : // @Range: 0 500
58 : // @Increment: 1
59 : // @User: Advanced
60 : AP_GROUPINFO("EXPIRE", 2, AP_OADatabase, _database_expiry_seconds, AP_OADATABASE_TIMEOUT_SECONDS_DEFAULT),
61 :
62 : // @Param: QUEUE_SIZE
63 : // @DisplayName: OADatabase queue maximum number of points
64 : // @Description: OADatabase queue maximum number of points. This in an input buffer size. Larger means it can handle larger bursts of incoming data points to filter into the database. No impact on cpu, only RAM. Recommend larger for faster datalinks or for sensors that generate a lot of data.
65 : // @Range: 1 200
66 : // @User: Advanced
67 : // @RebootRequired: True
68 : AP_GROUPINFO("QUEUE_SIZE", 3, AP_OADatabase, _queue_size_param, AP_OADATABASE_QUEUE_SIZE_DEFAULT),
69 :
70 : // @Param: OUTPUT
71 : // @DisplayName: OADatabase output level
72 : // @Description: OADatabase output level to configure which database objects are sent to the ground station. All data is always available internally for avoidance algorithms.
73 : // @Values: 0:Disabled,1:Send only HIGH importance items,2:Send HIGH and NORMAL importance items,3:Send all items
74 : // @User: Advanced
75 : AP_GROUPINFO("OUTPUT", 4, AP_OADatabase, _output_level, (float)OutputLevel::HIGH),
76 :
77 : // @Param: BEAM_WIDTH
78 : // @DisplayName: OADatabase beam width
79 : // @Description: Beam width of incoming lidar data, used to calculate a object radius if none is provided by the data source.
80 : // @Units: deg
81 : // @Range: 1 10
82 : // @User: Advanced
83 : // @RebootRequired: True
84 : AP_GROUPINFO("BEAM_WIDTH", 5, AP_OADatabase, _beam_width, 5.0f),
85 :
86 : // @Param: RADIUS_MIN
87 : // @DisplayName: OADatabase Minimum radius
88 : // @Description: Minimum radius of objects held in database
89 : // @Units: m
90 : // @Range: 0 10
91 : // @User: Advanced
92 : AP_GROUPINFO("RADIUS_MIN", 6, AP_OADatabase, _radius_min, 0.01f),
93 :
94 : // @Param: DIST_MAX
95 : // @DisplayName: OADatabase Distance Maximum
96 : // @Description: Maximum distance of objects held in database. Set to zero to disable the limits
97 : // @Units: m
98 : // @Range: 0 10
99 : // @User: Advanced
100 : AP_GROUPINFO("DIST_MAX", 7, AP_OADatabase, _dist_max, 0.0f),
101 :
102 : // @Param{Copter}: ALT_MIN
103 : // @DisplayName: OADatabase minimum altitude above home before storing obstacles
104 : // @Description: OADatabase will reject obstacles if vehicle's altitude above home is below this parameter, in a 3 meter radius around home. Set 0 to disable this feature.
105 : // @Units: m
106 : // @Range: 0 4
107 : // @User: Advanced
108 : AP_GROUPINFO_FRAME("ALT_MIN", 8, AP_OADatabase, _min_alt, 0.0f, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER),
109 :
110 : AP_GROUPEND
111 : };
112 :
113 1286 : AP_OADatabase::AP_OADatabase()
114 : {
115 1286 : if (_singleton != nullptr) {
116 0 : AP_HAL::panic("AP_OADatabase must be singleton");
117 : }
118 1286 : _singleton = this;
119 :
120 1286 : AP_Param::setup_object_defaults(this, var_info);
121 1286 : }
122 :
123 9 : void AP_OADatabase::init()
124 : {
125 : // PARAMETER_CONVERSION - Added: Jun-2025 for ArduPilot-4.7
126 9 : _database_expiry_seconds.convert_parameter_width(AP_PARAM_INT8);
127 :
128 9 : init_database();
129 9 : init_queue();
130 :
131 : // initialise scalar using beam width of at least 1deg
132 9 : dist_to_radius_scalar = tanf(radians(MAX(_beam_width, 1.0f)));
133 :
134 9 : if (!healthy()) {
135 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DB init failed . Sizes queue:%u, db:%u", (unsigned int)_queue.size, (unsigned int)_database.size);
136 0 : delete _queue.items;
137 0 : delete[] _database.items;
138 0 : return;
139 : }
140 : }
141 :
142 780 : void AP_OADatabase::update()
143 : {
144 780 : if (!healthy()) {
145 0 : return;
146 : }
147 :
148 780 : process_queue();
149 780 : database_items_remove_all_expired();
150 : }
151 :
152 : // Push an object into the database. Pos is the offset in meters from the EKF origin, measurement timestamp in ms, distance in meters
153 1736 : void AP_OADatabase::queue_push(const Vector3f &pos, const uint32_t timestamp_ms, const float distance, const OA_DbItem::Source source, const uint32_t id)
154 : {
155 : // Push with radius calculated from beam width
156 1736 : queue_push(pos, timestamp_ms, distance, distance * dist_to_radius_scalar, source, id);
157 1736 : }
158 :
159 : // Push an object into the database. Pos is the offset in meters from the EKF origin, measurement timestamp in ms, distance in meters, radius in meters
160 1736 : void AP_OADatabase::queue_push(const Vector3f &pos, const uint32_t timestamp_ms, const float distance, float radius, const OA_DbItem::Source source, const uint32_t id)
161 : {
162 1736 : if (!healthy()) {
163 0 : return;
164 : }
165 :
166 : // check if this obstacle needs to be rejected from DB because of low altitude near home
167 : #if APM_BUILD_COPTER_OR_HELI
168 1736 : if (!is_zero(_min_alt)) {
169 0 : Vector3f current_pos;
170 0 : if (!AP::ahrs().get_relative_position_NED_home(current_pos)) {
171 : // we do not know where the vehicle is
172 0 : return;
173 : }
174 0 : if (current_pos.xy().length() < AP_OADATABASE_DISTANCE_FROM_HOME) {
175 : // vehicle is within a small radius of home
176 0 : if (-current_pos.z < _min_alt) {
177 : // vehicle is below the minimum alt
178 0 : return;
179 : }
180 : }
181 : }
182 : #endif
183 :
184 : // Apply min radius parameter
185 1736 : radius = MAX(_radius_min, radius);
186 :
187 : // ignore objects that outside of the max distance
188 1736 : if (is_positive(_dist_max)) {
189 0 : const float closest_point = distance - radius;
190 0 : if (closest_point > _dist_max) {
191 0 : return;
192 : }
193 : }
194 :
195 1736 : const OA_DbItem item = {pos, timestamp_ms, radius, id, 0, AP_OADatabase::OA_DbItemImportance::Normal, source};
196 : {
197 1736 : WITH_SEMAPHORE(_queue.sem);
198 1736 : _queue.items->push(item);
199 1736 : }
200 : }
201 :
202 9 : void AP_OADatabase::init_queue()
203 : {
204 9 : _queue.size = _queue_size_param;
205 9 : if (_queue.size == 0) {
206 0 : return;
207 : }
208 :
209 9 : _queue.items = NEW_NOTHROW ObjectBuffer<OA_DbItem>(_queue.size);
210 9 : if (_queue.items != nullptr && _queue.items->get_size() == 0) {
211 : // allocation failed
212 0 : delete _queue.items;
213 0 : _queue.items = nullptr;
214 : }
215 : }
216 :
217 9 : void AP_OADatabase::init_database()
218 : {
219 9 : _database.size = _database_size_param;
220 9 : if (_database_size_param == 0) {
221 0 : return;
222 : }
223 :
224 909 : _database.items = NEW_NOTHROW OA_DbItem[_database.size];
225 : }
226 :
227 : // get bitmask of gcs channels item should be sent to based on its importance
228 : // returns 0xFF (send to all channels) if should be sent, 0 if it should not be sent
229 3001 : uint8_t AP_OADatabase::get_send_to_gcs_flags(const OA_DbItemImportance importance) const
230 : {
231 3001 : switch (importance) {
232 0 : case OA_DbItemImportance::Low:
233 0 : if (_output_level >= OutputLevel::ALL) {
234 0 : return 0xFF;
235 : }
236 0 : break;
237 :
238 3001 : case OA_DbItemImportance::Normal:
239 3001 : if (_output_level >= OutputLevel::HIGH_AND_NORMAL) {
240 3001 : return 0xFF;
241 : }
242 0 : break;
243 :
244 0 : case OA_DbItemImportance::High:
245 0 : if (_output_level >= OutputLevel::HIGH) {
246 0 : return 0xFF;
247 : }
248 0 : break;
249 : }
250 0 : return 0x0;
251 : }
252 :
253 : // Return true if item A is likely the same as item B
254 16859 : bool AP_OADatabase::item_match(const OA_DbItem& A, const OA_DbItem& B) const
255 : {
256 : // Items must be from the same source to match
257 16859 : if (A.source != B.source) {
258 0 : return false;
259 : }
260 :
261 16859 : switch (A.source) {
262 0 : case OA_DbItem::Source::AIS:
263 : // Check IDs
264 0 : return A.id == B.id;
265 :
266 16859 : case OA_DbItem::Source::proximity:
267 : // Check if close
268 16859 : const float distance_sq = (A.pos - B.pos).length_squared();
269 16859 : return distance_sq < sq(MAX(A.radius, B.radius));
270 : }
271 :
272 0 : return false;
273 : }
274 :
275 : // returns true when there's more work in the queue to do
276 37797 : bool AP_OADatabase::process_queue()
277 : {
278 37797 : if (!healthy()) {
279 0 : return false;
280 : }
281 :
282 : // processing queue by moving those entries into the database
283 : // Using a for with fixed size is better than while(!empty) because the
284 : // while could get us stuck here longer than expected if we're getting
285 : // a lot of values pushing into it while we're trying to empty it. With
286 : // the for we know we will exit at an expected time
287 37797 : const uint16_t queue_available = MIN(_queue.items->available(), 100U);
288 37797 : if (queue_available == 0) {
289 37369 : return false;
290 : }
291 :
292 2153 : for (uint16_t queue_index=0; queue_index<queue_available; queue_index++) {
293 1725 : OA_DbItem item;
294 :
295 : bool pop_success;
296 : {
297 1725 : WITH_SEMAPHORE(_queue.sem);
298 1725 : pop_success = _queue.items->pop(item);
299 1725 : }
300 1725 : if (!pop_success) {
301 0 : return false;
302 : }
303 :
304 1725 : item.send_to_gcs = get_send_to_gcs_flags(item.importance);
305 :
306 : // compare item to all items in database. If found a similar item, update the existing, else add it as a new one
307 1725 : bool found = false;
308 16980 : for (uint16_t i=0; i<_database.count; i++) {
309 16859 : if (item_match(_database.items[i], item)) {
310 1604 : database_item_refresh(_database.items[i], item);
311 1604 : found = true;
312 1604 : break;
313 : }
314 : }
315 :
316 1725 : if (!found) {
317 121 : database_item_add(item);
318 : }
319 : }
320 428 : return (_queue.items->available() > 0);
321 : }
322 :
323 121 : void AP_OADatabase::database_item_add(const OA_DbItem &item)
324 : {
325 121 : if (_database.count >= _database.size) {
326 0 : return;
327 : }
328 121 : _database.items[_database.count] = item;
329 121 : _database.items[_database.count].send_to_gcs = get_send_to_gcs_flags(_database.items[_database.count].importance);
330 121 : _database.count++;
331 : }
332 :
333 0 : void AP_OADatabase::database_item_remove(const uint16_t index)
334 : {
335 0 : if (index >= _database.count || _database.count == 0) {
336 : // index out of range
337 0 : return;
338 : }
339 :
340 : // radius of 0 tells the GCS we don't care about it any more (aka it expired)
341 0 : _database.items[index].radius = 0;
342 0 : _database.items[index].send_to_gcs = get_send_to_gcs_flags(_database.items[index].importance);
343 :
344 0 : _database.count--;
345 0 : if (_database.count == 0) {
346 0 : return;
347 : }
348 :
349 0 : if (index != _database.count) {
350 : // copy last object in array over expired object
351 0 : _database.items[index] = _database.items[_database.count];
352 0 : _database.items[index].send_to_gcs = get_send_to_gcs_flags(_database.items[index].importance);
353 : }
354 : }
355 :
356 1604 : void AP_OADatabase::database_item_refresh(OA_DbItem ¤t_item, const OA_DbItem &new_item) const
357 : {
358 : const bool is_different =
359 2103 : (!is_equal(current_item.radius, new_item.radius)) ||
360 499 : (new_item.timestamp_ms - current_item.timestamp_ms >= 500);
361 :
362 1604 : if (is_different) {
363 : // update timestamp and radius on close object so it stays around longer
364 : // and trigger resending to GCS
365 1155 : current_item.timestamp_ms = new_item.timestamp_ms;
366 1155 : current_item.radius = new_item.radius;
367 1155 : current_item.send_to_gcs = get_send_to_gcs_flags(current_item.importance);
368 :
369 1155 : if (current_item.source == OA_DbItem::Source::AIS) {
370 : // Update position for AIS items, these tend to be large and update slowly
371 0 : current_item.pos = new_item.pos;
372 : }
373 : }
374 1604 : }
375 :
376 780 : void AP_OADatabase::database_items_remove_all_expired()
377 : {
378 : // calculate age of all items in the _database
379 :
380 780 : if (_database_expiry_seconds <= 0) {
381 : // zero means never expire. This is not normal behavior but perhaps you could send a static
382 : // environment once that you don't want to have to constantly update
383 0 : return;
384 : }
385 :
386 780 : const uint32_t now_ms = AP_HAL::millis();
387 780 : const uint32_t expiry_ms = (uint32_t)_database_expiry_seconds * 1000;
388 780 : uint16_t index = 0;
389 1041 : while (index < _database.count) {
390 261 : if (now_ms - _database.items[index].timestamp_ms > expiry_ms) {
391 0 : database_item_remove(index);
392 : } else {
393 261 : index++;
394 : }
395 : }
396 : }
397 :
398 : #if HAL_GCS_ENABLED
399 : // send ADSB_VEHICLE mavlink messages
400 510114 : void AP_OADatabase::send_adsb_vehicle(mavlink_channel_t chan, uint16_t interval_ms)
401 : {
402 : // ensure database's send_to_gcs field is large enough
403 : static_assert(MAVLINK_COMM_NUM_BUFFERS <= sizeof(OA_DbItem::send_to_gcs) * 8,
404 : "AP_OADatabase's OA_DBItem.send_to_gcs bitmask must be large enough to hold MAVLINK_COMM_NUM_BUFFERS");
405 :
406 510114 : if ((_output_level <= OutputLevel::NONE) || !healthy()) {
407 501802 : return;
408 : }
409 :
410 8358 : const uint8_t chan_as_bitmask = 1 << chan;
411 8358 : const char callsign[9] = "OA_DB";
412 :
413 : // calculate how many messages we should send
414 8358 : const uint32_t now_ms = AP_HAL::millis();
415 8358 : uint16_t num_to_send = 1;
416 8358 : uint16_t num_sent = 0;
417 8358 : if ((_last_send_to_gcs_ms[chan] != 0) && (interval_ms > 0)) {
418 8349 : uint32_t diff_ms = now_ms - _last_send_to_gcs_ms[chan];
419 8349 : num_to_send = MAX(diff_ms / interval_ms, 1U);
420 : }
421 8358 : _last_send_to_gcs_ms[chan] = now_ms;
422 :
423 : // send unsent objects until output buffer is full or have sent enough
424 8424 : for (uint16_t i=0; i < _database.count; i++) {
425 112 : if (!HAVE_PAYLOAD_SPACE(chan, ADSB_VEHICLE) || (num_sent >= num_to_send)) {
426 : // all done for now
427 46 : return;
428 : }
429 :
430 66 : const uint16_t idx = _next_index_to_send[chan];
431 :
432 : // prepare to send next object
433 66 : _next_index_to_send[chan]++;
434 66 : if (_next_index_to_send[chan] >= _database.count) {
435 20 : _next_index_to_send[chan] = 0;
436 : }
437 :
438 66 : if ((_database.items[idx].send_to_gcs & chan_as_bitmask) == 0) {
439 14 : continue;
440 : }
441 :
442 : // convert object's position as an offset from EKF origin to Location
443 52 : const Location item_loc(Vector3f{_database.items[idx].pos.x * 100.0f, _database.items[idx].pos.y * 100.0f, _database.items[idx].pos.z * 100.0f}, Location::AltFrame::ABOVE_ORIGIN);
444 :
445 52 : mavlink_msg_adsb_vehicle_send(chan,
446 : idx,
447 52 : item_loc.lat,
448 52 : item_loc.lng,
449 : 0, // altitude_type
450 52 : item_loc.alt,
451 : 0, // heading
452 : 0, // hor_velocity
453 : 0, // ver_velocity
454 : callsign, // callsign
455 : 255, // emitter_type
456 : 0, // tslc
457 : 0, // flags
458 52 : (uint16_t)(_database.items[idx].radius * 100.f)); // squawk
459 :
460 : // unmark item for sending to gcs
461 52 : _database.items[idx].send_to_gcs &= ~chan_as_bitmask;
462 :
463 : // update highest index sent to GCS
464 52 : _highest_index_sent[chan] = MAX(idx, _highest_index_sent[chan]);
465 :
466 : // update count sent
467 52 : num_sent++;
468 : }
469 :
470 : // clear expired items in case the database size shrank
471 8312 : while (_highest_index_sent[chan] > _database.count) {
472 0 : if (!HAVE_PAYLOAD_SPACE(chan, ADSB_VEHICLE) || (num_sent >= num_to_send)) {
473 : // all done for now
474 0 : return;
475 : }
476 :
477 0 : const uint16_t idx = _highest_index_sent[chan];
478 0 : _highest_index_sent[chan]--;
479 :
480 0 : if (_database.items[idx].importance != OA_DbItemImportance::High) {
481 0 : continue;
482 : }
483 :
484 0 : mavlink_msg_adsb_vehicle_send(chan,
485 : idx, // id
486 : 0, // latitude
487 : 0, // longitude
488 : 0, // altitude_type
489 : 0, // altitude
490 : 0, // heading
491 : 0, // hor_velocity
492 : 0, // ver_velocity
493 : callsign, // callsign
494 : 255, // emitter_type
495 : 0, // tslc
496 : 0, // flags
497 : 0); // squawk
498 :
499 : // update count sent
500 0 : num_sent++;
501 : }
502 : }
503 : #endif // HAL_GCS_ENABLED
504 :
505 : // singleton instance
506 : AP_OADatabase *AP_OADatabase::_singleton;
507 :
508 : namespace AP {
509 467660 : AP_OADatabase *oadatabase()
510 : {
511 467660 : return AP_OADatabase::get_singleton();
512 : }
513 :
514 : }
515 :
516 : #endif // AP_OADATABASE_ENABLED
|
