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 : /*
17 : AP_ADSB.cpp
18 :
19 : ADS-B RF based collision avoidance module
20 : https://en.wikipedia.org/wiki/Automatic_dependent_surveillance_%E2%80%93_broadcast
21 : */
22 :
23 : #include "AP_ADSB_config.h"
24 :
25 : #if HAL_ADSB_ENABLED
26 :
27 : #include "AP_ADSB.h"
28 :
29 : #include "AP_ADSB_MAVLink.h"
30 : #include "AP_ADSB_uAvionix_UCP.h"
31 : #include "AP_ADSB_Sagetech.h"
32 : #include "AP_ADSB_Sagetech_MXS.h"
33 :
34 : #include <AP_AHRS/AP_AHRS.h>
35 : #include <AP_Baro/AP_Baro.h>
36 : #include <AP_GPS/AP_GPS.h>
37 : #include <AP_Logger/AP_Logger.h>
38 : #include <AP_Vehicle/AP_Vehicle_Type.h>
39 : #include <GCS_MAVLink/GCS.h>
40 : #include <AP_RTC/AP_RTC.h>
41 :
42 : #define VEHICLE_TIMEOUT_MS 5000 // if no updates in this time, drop it from the list
43 : #define ADSB_SQUAWK_OCTAL_DEFAULT 1200
44 :
45 : #ifndef ADSB_VEHICLE_LIST_SIZE_DEFAULT
46 : #define ADSB_VEHICLE_LIST_SIZE_DEFAULT 25
47 : #endif
48 :
49 : #ifndef ADSB_LIST_RADIUS_DEFAULT
50 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
51 : #define ADSB_LIST_RADIUS_DEFAULT 10000 // in meters
52 : #else
53 : #define ADSB_LIST_RADIUS_DEFAULT 2000 // in meters
54 : #endif
55 : #endif
56 :
57 : #ifndef AP_ADSB_TYPE_DEFAULT
58 : #define AP_ADSB_TYPE_DEFAULT 0
59 : #endif
60 :
61 : extern const AP_HAL::HAL& hal;
62 :
63 : AP_ADSB *AP_ADSB::_singleton;
64 :
65 : // table of user settable parameters
66 : const AP_Param::GroupInfo AP_ADSB::var_info[] = {
67 : // @Param: TYPE
68 : // @DisplayName: ADSB Type
69 : // @Description: Type of ADS-B hardware for ADSB-in and ADSB-out configuration and operation. Incoming MAVLink ADSB_VEHICLE messages are processed for any non-zero type. Select 1 (MAVLink) either for a MAVLink-connected transceiver such as the uAvionix ping, or to process ADSB_VEHICLE messages forwarded from a companion computer with no ADS-B hardware attached. ADSB-out is only sent once a transceiver reports its health, so with no hardware attached this type is receive-only.
70 : // @Values: 0:Disabled,1:MAVLink,2:Sagetech,3:uAvionix-UCP,4:Sagetech MX Series
71 : // @User: Standard
72 : // @RebootRequired: True
73 : AP_GROUPINFO_FLAGS("TYPE", 0, AP_ADSB, _type[0], AP_ADSB_TYPE_DEFAULT, AP_PARAM_FLAG_ENABLE),
74 :
75 : // index 1 is reserved - was BEHAVIOR
76 :
77 : // @Param: LIST_MAX
78 : // @DisplayName: ADSB vehicle list size
79 : // @Description: ADSB list size of nearest vehicles. Longer lists take longer to refresh with lower SRx_ADSB values.
80 : // @Range: 1 100
81 : // @User: Advanced
82 : // @RebootRequired: True
83 : AP_GROUPINFO("LIST_MAX", 2, AP_ADSB, in_state.list_size_param, ADSB_VEHICLE_LIST_SIZE_DEFAULT),
84 :
85 :
86 : // @Param: LIST_RADIUS
87 : // @DisplayName: ADSB vehicle list radius filter
88 : // @Description: ADSB vehicle list radius filter. Vehicles detected outside this radius will be completely ignored. They will not show up in the SRx_ADSB stream to the GCS and will not be considered in any avoidance calculations. A value of 0 will disable this filter.
89 : // @Range: 0 100000
90 : // @User: Advanced
91 : // @Units: m
92 : AP_GROUPINFO("LIST_RADIUS", 3, AP_ADSB, in_state.list_radius, ADSB_LIST_RADIUS_DEFAULT),
93 :
94 : // @Param: ICAO_ID
95 : // @DisplayName: ICAO_ID vehicle identification number
96 : // @Description: ICAO_ID unique vehicle identification number of this aircraft. This is an integer limited to 24bits. If set to 0 then one will be randomly generated. If set to -1 then static information is not sent, transceiver is assumed pre-programmed.
97 : // @Range: -1 16777215
98 : // @User: Advanced
99 : AP_GROUPINFO("ICAO_ID", 4, AP_ADSB, out_state.cfg.ICAO_id_param, 0),
100 :
101 : // @Param: EMIT_TYPE
102 : // @DisplayName: Emitter type
103 : // @Description: ADSB classification for the type of vehicle emitting the transponder signal. Default value is 14 (UAV).
104 : // @Values: 0:NoInfo,1:Light,2:Small,3:Large,4:HighVortexlarge,5:Heavy,6:HighlyManuv,7:Rotocraft,8:RESERVED,9:Glider,10:LightAir,11:Parachute,12:UltraLight,13:RESERVED,14:UAV,15:Space,16:RESERVED,17:EmergencySurface,18:ServiceSurface,19:PointObstacle
105 : // @User: Advanced
106 : AP_GROUPINFO("EMIT_TYPE", 5, AP_ADSB, out_state.cfg.emitterType, ADSB_EMITTER_TYPE_UAV),
107 :
108 : // @Param: LEN_WIDTH
109 : // @DisplayName: Aircraft length and width
110 : // @Description: Aircraft length and width dimension options in Length and Width in meters. In most cases, use a value of 1 for smallest size.
111 : // @Values: 0:NO_DATA,1:L15W23,2:L25W28P5,3:L25W34,4:L35W33,5:L35W38,6:L45W39P5,7:L45W45,8:L55W45,9:L55W52,10:L65W59P5,11:L65W67,12:L75W72P5,13:L75W80,14:L85W80,15:L85W90
112 : // @User: Advanced
113 : AP_GROUPINFO("LEN_WIDTH", 6, AP_ADSB, out_state.cfg.lengthWidth, UAVIONIX_ADSB_OUT_CFG_AIRCRAFT_SIZE_L15M_W23M),
114 :
115 : // @Param: OFFSET_LAT
116 : // @DisplayName: GPS antenna lateral offset
117 : // @Description: GPS antenna lateral offset. This describes the physical location offset from center of the GPS antenna on the aircraft.
118 : // @Values: 0:NoData,1:Left2m,2:Left4m,3:Left6m,4:Center,5:Right2m,6:Right4m,7:Right6m
119 : // @User: Advanced
120 : AP_GROUPINFO("OFFSET_LAT", 7, AP_ADSB, out_state.cfg.gpsOffsetLat, UAVIONIX_ADSB_OUT_CFG_GPS_OFFSET_LAT_RIGHT_0M),
121 :
122 : // @Param: OFFSET_LON
123 : // @DisplayName: GPS antenna longitudinal offset
124 : // @Description: GPS antenna longitudinal offset. This is usually set to 1, Applied By Sensor
125 : // @Values: 0:NO_DATA,1:AppliedBySensor
126 : // @User: Advanced
127 : AP_GROUPINFO("OFFSET_LON", 8, AP_ADSB, out_state.cfg.gpsOffsetLon, UAVIONIX_ADSB_OUT_CFG_GPS_OFFSET_LON_APPLIED_BY_SENSOR),
128 :
129 : // @Param: RF_SELECT
130 : // @DisplayName: Transceiver RF selection
131 : // @Description: Transceiver RF selection for Rx enable and/or Tx enable. This only effects devices that can Tx and/or Rx. Rx-only devices should override this to always be Rx-only.
132 : // @Bitmask: 0:Rx,1:Tx
133 : // @User: Advanced
134 : AP_GROUPINFO("RF_SELECT", 9, AP_ADSB, out_state.cfg.rfSelect, UAVIONIX_ADSB_OUT_RF_SELECT_RX_ENABLED),
135 :
136 : // @Param: SQUAWK
137 : // @DisplayName: Squawk code
138 : // @Description: VFR squawk (Mode 3/A) code is a pre-programmed default code when the pilot is flying VFR and not in contact with ATC. In the USA, the VFR squawk code is octal 1200 (hex 0x280, decimal 640) and in most parts of Europe the VFR squawk code is octal 7000. If an invalid octal number is set then it will be reset to 1200.
139 : // @Range: 0 7777
140 : // @Units: octal
141 : // @User: Advanced
142 : AP_GROUPINFO("SQUAWK", 10, AP_ADSB, out_state.cfg.squawk_octal_param, ADSB_SQUAWK_OCTAL_DEFAULT),
143 :
144 : // @Param: RF_CAPABLE
145 : // @DisplayName: RF capabilities
146 : // @Description: Describes your hardware RF In/Out capabilities.
147 : // @Bitmask: 0:UAT_in,1:1090ES_in,2:UAT_out,3:1090ES_out
148 : // @User: Advanced
149 : AP_GROUPINFO("RF_CAPABLE", 11, AP_ADSB, out_state.cfg.rf_capable, 0),
150 :
151 : // @Param: LIST_ALT
152 : // @DisplayName: ADSB vehicle list altitude filter
153 : // @Description: ADSB vehicle list altitude filter. Vehicles detected more than this altitude above our own altitude will be completely ignored. They will not show up in the SRx_ADSB stream to the GCS and will not be considered in any avoidance calculations. A value of 0 will disable this filter.
154 : // @Range: 0 32767
155 : // @User: Advanced
156 : // @Units: m
157 : AP_GROUPINFO("LIST_ALT", 12, AP_ADSB, in_state.list_altitude, 0),
158 :
159 : // @Param: ICAO_SPECL
160 : // @DisplayName: ICAO_ID of special vehicle
161 : // @Description: ICAO_ID of special vehicle that ignores ADSB_LIST_RADIUS and ADSB_LIST_ALT. The vehicle is always tracked. Use 0 to disable.
162 : // @User: Advanced
163 : AP_GROUPINFO("ICAO_SPECL", 13, AP_ADSB, _special_ICAO_target, 0),
164 :
165 : // @Param: LOG
166 : // @DisplayName: ADS-B logging
167 : // @Description: 0: no logging, 1: log only special ID, 2:log all
168 : // @Values: 0:no logging,1:log only special ID,2:log all
169 : // @User: Advanced
170 : AP_GROUPINFO("LOG", 14, AP_ADSB, _log, 1),
171 :
172 : // @Param: OPTIONS
173 : // @DisplayName: ADS-B Options
174 : // @Description: Options for emergency failsafe codes and device capabilities
175 : // @Bitmask: 0:Ping200X Send GPS,1:Squawk 7400 on RC failsafe,2:Squawk 7400 on GCS failsafe,3:Sagetech MXS use External Config,4:Transmit in traditional Mode 3A/C only and inhibit Mode-S and ES (ADSB) transmissions
176 : // @User: Advanced
177 : AP_GROUPINFO("OPTIONS", 15, AP_ADSB, _options, 0),
178 :
179 : AP_GROUPEND
180 : };
181 :
182 : // constructor
183 3386 : AP_ADSB::AP_ADSB()
184 : {
185 1693 : AP_Param::setup_object_defaults(this, var_info);
186 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
187 1693 : if (_singleton != nullptr) {
188 0 : AP_HAL::panic("AP_ADSB must be singleton");
189 : }
190 : #endif
191 1693 : _singleton = this;
192 :
193 : #ifdef ADSB_STATIC_CALLSIGN
194 : strncpy(out_state.cfg.callsign, ADSB_STATIC_CALLSIGN, sizeof(out_state.cfg.callsign));
195 : #endif
196 1693 : }
197 :
198 : /*
199 : * Initialize variables and allocate memory for array
200 : */
201 10 : void AP_ADSB::init(void)
202 : {
203 10 : if (in_state.vehicle_list == nullptr) {
204 : // sanity check param
205 10 : in_state.list_size_param.set(constrain_int16(in_state.list_size_param, 1, INT16_MAX));
206 :
207 10 : in_state.vehicle_list = NEW_NOTHROW adsb_vehicle_t[in_state.list_size_param];
208 :
209 10 : if (in_state.vehicle_list == nullptr) {
210 : // dynamic RAM allocation of in_state.vehicle_list[] failed
211 0 : _init_failed = true; // this keeps us from constantly trying to init forever in main update
212 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ADSB: Unable to initialize ADSB vehicle list");
213 0 : return;
214 : }
215 10 : in_state.list_size_allocated = in_state.list_size_param;
216 : }
217 :
218 10 : if (detected_num_instances == 0) {
219 20 : for (uint8_t i=0; i<ADSB_MAX_INSTANCES; i++) {
220 10 : detect_instance(i);
221 10 : if (_backend[i] == nullptr) {
222 0 : continue;
223 : }
224 10 : if (!_backend[i]->init()) {
225 0 : delete _backend[i];
226 0 : _backend[i] = nullptr;
227 0 : continue;
228 : }
229 : // success
230 10 : detected_num_instances = i+1;
231 : }
232 : }
233 :
234 10 : if (detected_num_instances == 0) {
235 0 : _init_failed = true;
236 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ADSB: Unable to initialize ADSB driver");
237 : }
238 : }
239 :
240 2254270 : bool AP_ADSB::check_startup()
241 : {
242 2254270 : if (_init_failed) {
243 0 : return false;
244 : }
245 :
246 2254270 : bool all_backends_disabled = true;
247 4494511 : for (uint8_t instance=0; instance<ADSB_MAX_INSTANCES; instance++) {
248 2254270 : if (_type[instance] > 0) {
249 14029 : all_backends_disabled = false;
250 14029 : break;
251 : }
252 : }
253 :
254 2254270 : if (all_backends_disabled) {
255 : // nothing to do
256 2240241 : return false;
257 : }
258 14029 : if (in_state.vehicle_list == nullptr) {
259 10 : init();
260 : }
261 14029 : return in_state.vehicle_list != nullptr;
262 : }
263 :
264 :
265 : // detect if an instance of an ADSB sensor is connected.
266 10 : void AP_ADSB::detect_instance(uint8_t instance)
267 : {
268 10 : switch (get_type(instance)) {
269 0 : case Type::None:
270 0 : return;
271 :
272 8 : case Type::MAVLink:
273 : #if HAL_ADSB_MAVLINK_ENABLED
274 8 : if (AP_ADSB_MAVLink::detect()) {
275 8 : _backend[instance] = NEW_NOTHROW AP_ADSB_MAVLink(*this, instance);
276 : }
277 : #endif
278 8 : break;
279 :
280 0 : case Type::uAvionix_UCP:
281 : #if HAL_ADSB_UCP_ENABLED
282 0 : if (AP_ADSB_uAvionix_UCP::detect()) {
283 0 : _backend[instance] = NEW_NOTHROW AP_ADSB_uAvionix_UCP(*this, instance);
284 : }
285 : #endif
286 0 : break;
287 :
288 0 : case Type::Sagetech:
289 : #if HAL_ADSB_SAGETECH_ENABLED
290 0 : if (AP_ADSB_Sagetech::detect()) {
291 0 : _backend[instance] = NEW_NOTHROW AP_ADSB_Sagetech(*this, instance);
292 : }
293 : #endif
294 0 : break;
295 :
296 2 : case Type::Sagetech_MXS:
297 : #if HAL_ADSB_SAGETECH_MXS_ENABLED
298 2 : if (AP_ADSB_Sagetech_MXS::detect()) {
299 2 : _backend[instance] = NEW_NOTHROW AP_ADSB_Sagetech_MXS(*this, instance);
300 : }
301 : #endif
302 2 : break;
303 : }
304 :
305 : }
306 :
307 : // get instance type from instance
308 10 : AP_ADSB::Type AP_ADSB::get_type(uint8_t instance) const
309 : {
310 10 : if (instance < ADSB_MAX_INSTANCES) {
311 10 : return (Type)(_type[instance].get());
312 : }
313 0 : return Type::None;
314 : }
315 :
316 60 : bool AP_ADSB::is_valid_callsign(uint16_t octal)
317 : {
318 : // treat "octal" as decimal and test if any decimal digit is > 7
319 60 : if (octal > 7777) {
320 12 : return false;
321 : }
322 :
323 150 : while (octal != 0) {
324 110 : if (octal % 10 > 7) {
325 8 : return false;
326 : }
327 102 : octal /= 10;
328 : }
329 :
330 40 : return true;
331 : }
332 :
333 : #if AP_GPS_ENABLED && AP_AHRS_ENABLED && AP_BARO_ENABLED
334 : /*
335 : * periodic update to handle vehicle timeouts and trigger collision detection
336 : */
337 1306596 : void AP_ADSB::update(void)
338 : {
339 1306596 : Loc loc{};
340 1306596 : if (!AP::ahrs().get_location(loc)) {
341 90888 : loc.zero();
342 : }
343 :
344 1306596 : const AP_GPS &gps = AP::gps();
345 :
346 1306596 : loc.fix_type = gps.status();
347 1306596 : loc.epoch_us = gps.time_epoch_usec();
348 : #if AP_RTC_ENABLED
349 1306596 : loc.have_epoch_from_rtc_us = AP::rtc().get_utc_usec(loc.epoch_from_rtc_us);
350 : #endif
351 :
352 1306596 : loc.satellites = gps.num_sats();
353 :
354 1306596 : loc.horizontal_pos_accuracy_is_valid = gps.horizontal_accuracy(loc.horizontal_pos_accuracy);
355 1306596 : loc.vertical_pos_accuracy_is_valid = gps.vertical_accuracy(loc.vertical_pos_accuracy);
356 1306596 : loc.horizontal_vel_accuracy_is_valid = gps.speed_accuracy(loc.horizontal_vel_accuracy);
357 :
358 :
359 1306596 : loc.vel_ned = gps.velocity();
360 :
361 1306596 : loc.vertRateD_is_valid = AP::ahrs().get_vert_pos_rate_D(loc.vertRateD);
362 :
363 1306596 : const auto &baro = AP::baro();
364 1306596 : loc.baro_is_healthy = baro.healthy();
365 :
366 : // Altitude difference between sea level pressure and current
367 : // pressure (in metres)
368 1306596 : if (loc.baro_is_healthy) {
369 1306085 : loc.baro_alt_press_diff_sea_level = baro.get_altitude_difference(SSL_AIR_PRESSURE, baro.get_pressure());
370 : }
371 :
372 1306596 : update(loc);
373 1306596 : }
374 : #endif // AP_GPS_ENABLED && AP_AHRS_ENABLED
375 :
376 1306596 : void AP_ADSB::update(const AP_ADSB::Loc &loc)
377 : {
378 1306596 : if (!check_startup()) {
379 1303679 : return;
380 : }
381 :
382 2917 : _my_loc = loc;
383 :
384 2917 : const uint32_t now = AP_HAL::millis();
385 :
386 : // check current list for vehicles that time out
387 2917 : uint16_t index = 0;
388 3640 : while (index < in_state.vehicle_count) {
389 : // check list and drop stale vehicles. When disabled, the list will get flushed
390 723 : if (now - in_state.vehicle_list[index].last_update_ms > VEHICLE_TIMEOUT_MS) {
391 : // don't increment index, we want to check this same index again because the contents changed
392 : // also, if we're disabled then clear the list
393 5 : delete_vehicle(index);
394 : } else {
395 718 : index++;
396 : }
397 : }
398 :
399 2917 : if (out_state.cfg.squawk_octal_param != out_state.cfg.squawk_octal) {
400 : // param changed, check that it's a valid octal
401 10 : if (!is_valid_callsign(out_state.cfg.squawk_octal_param)) {
402 : // invalid, reset it to default
403 0 : out_state.cfg.squawk_octal_param.set(ADSB_SQUAWK_OCTAL_DEFAULT);
404 : }
405 10 : out_state.cfg.squawk_octal = (uint16_t)out_state.cfg.squawk_octal_param;
406 : }
407 :
408 : // ensure it's positive 24bit but allow -1
409 2917 : if (out_state.cfg.ICAO_id_param <= -1 || out_state.cfg.ICAO_id_param > 0x00FFFFFF) {
410 : // icao param of -1 means static information is not sent, transceiver is assumed pre-programmed.
411 : // reset timer constantly so it never reaches 10s so it never sends
412 0 : out_state.last_config_ms = now;
413 :
414 2917 : } else if ((out_state.cfg.rfSelect & UAVIONIX_ADSB_OUT_RF_SELECT_TX_ENABLED) &&
415 0 : (out_state.cfg.ICAO_id == 0 || out_state.cfg.ICAO_id_param_prev != out_state.cfg.ICAO_id_param)) {
416 :
417 : // if param changed then regenerate. This allows the param to be changed back to zero to trigger a re-generate
418 0 : if (out_state.cfg.ICAO_id_param == 0) {
419 0 : out_state.cfg.ICAO_id = genICAO(_my_loc);
420 : } else {
421 0 : out_state.cfg.ICAO_id = out_state.cfg.ICAO_id_param;
422 : }
423 0 : out_state.cfg.ICAO_id_param_prev = out_state.cfg.ICAO_id_param;
424 :
425 : #ifndef ADSB_STATIC_CALLSIGN
426 0 : if (!out_state.cfg.was_set_externally) {
427 0 : set_callsign("ARDU", true);
428 : }
429 : #endif
430 0 : out_state.last_config_ms = 0; // send now
431 : }
432 :
433 5834 : for (uint8_t i=0; i<detected_num_instances; i++) {
434 2917 : if (_backend[i] != nullptr && _type[i].get() != (int8_t)Type::None) {
435 2917 : _backend[i]->update();
436 : }
437 : }
438 :
439 : }
440 :
441 : /*
442 : * determine index and distance of furthest vehicle. This is
443 : * used to bump it off when a new closer aircraft is detected
444 : */
445 0 : void AP_ADSB::determine_furthest_aircraft(void)
446 : {
447 0 : float max_distance = 0;
448 0 : uint16_t max_distance_index = 0;
449 :
450 0 : for (uint16_t index = 0; index < in_state.vehicle_count; index++) {
451 0 : if (is_special_vehicle(in_state.vehicle_list[index].info.ICAO_address)) {
452 0 : continue;
453 : }
454 0 : const float distance = _my_loc.get_distance(get_location(in_state.vehicle_list[index]));
455 0 : if (max_distance < distance || index == 0) {
456 0 : max_distance = distance;
457 0 : max_distance_index = index;
458 : }
459 : } // for index
460 :
461 0 : in_state.furthest_vehicle_index = max_distance_index;
462 0 : in_state.furthest_vehicle_distance = max_distance;
463 0 : }
464 :
465 : /*
466 : * Convert/Extract a Location from a vehicle
467 : */
468 8398 : Location AP_ADSB::get_location(const adsb_vehicle_t &vehicle) const
469 : {
470 : const Location loc = Location(
471 8398 : vehicle.info.lat,
472 8398 : vehicle.info.lon,
473 8398 : vehicle.info.altitude * 0.1f,
474 8398 : Location::AltFrame::ABSOLUTE);
475 :
476 8398 : return loc;
477 : }
478 :
479 : /*
480 : * delete a vehicle by copying last vehicle to
481 : * current index then decrementing count
482 : */
483 6 : void AP_ADSB::delete_vehicle(const uint16_t index)
484 : {
485 6 : if (index >= in_state.vehicle_count) {
486 : // index out of range
487 0 : return;
488 : }
489 :
490 : // if the vehicle is the furthest, invalidate it. It has been bumped
491 6 : if (index == in_state.furthest_vehicle_index && in_state.furthest_vehicle_distance > 0) {
492 0 : in_state.furthest_vehicle_distance = 0;
493 0 : in_state.furthest_vehicle_index = 0;
494 : }
495 6 : if (index != (in_state.vehicle_count-1)) {
496 1 : in_state.vehicle_list[index] = in_state.vehicle_list[in_state.vehicle_count-1];
497 : }
498 : // TODO: is memset needed? When we decrement the index we essentially forget about it
499 6 : memset(&in_state.vehicle_list[in_state.vehicle_count-1], 0, sizeof(adsb_vehicle_t));
500 6 : in_state.vehicle_count--;
501 : }
502 :
503 : /*
504 : * Search _vehicle_list for the given vehicle. A match
505 : * depends on ICAO_address. Returns true if match found
506 : * and index is populated. otherwise, return false.
507 : */
508 16775 : bool AP_ADSB::find_index(const adsb_vehicle_t &vehicle, uint16_t *index) const
509 : {
510 51951 : for (uint16_t i = 0; i < in_state.vehicle_count; i++) {
511 51932 : if (in_state.vehicle_list[i].info.ICAO_address == vehicle.info.ICAO_address) {
512 16756 : *index = i;
513 16756 : return true;
514 : }
515 : }
516 19 : return false;
517 : }
518 :
519 : /*
520 : * Update the vehicle list. If the vehicle is already in the
521 : * list then it will update it, otherwise it will be added.
522 : */
523 8398 : void AP_ADSB::handle_adsb_vehicle(const adsb_vehicle_t &vehicle)
524 : {
525 8398 : if (!check_startup()) {
526 7 : return;
527 : }
528 :
529 8392 : uint16_t index = in_state.list_size_allocated + 1; // initialize with invalid index
530 8392 : const Location vehicle_loc = AP_ADSB::get_location(vehicle);
531 8392 : const bool my_loc_is_zero = _my_loc.is_zero();
532 8392 : const float my_loc_distance_to_vehicle = _my_loc.get_distance(vehicle_loc);
533 8392 : const bool is_special = is_special_vehicle(vehicle.info.ICAO_address);
534 8392 : const bool out_of_range = in_state.list_radius > 0 && !my_loc_is_zero && my_loc_distance_to_vehicle > in_state.list_radius && !is_special;
535 8392 : const bool out_of_range_alt = in_state.list_altitude > 0 && !my_loc_is_zero && abs(vehicle_loc.alt - _my_loc.alt) > in_state.list_altitude*100 && !is_special;
536 8392 : const bool is_tracked_in_list = find_index(vehicle, &index);
537 8392 : const uint32_t now = AP_HAL::millis();
538 :
539 8392 : const uint16_t required_flags_position = ADSB_FLAGS_VALID_COORDS | ADSB_FLAGS_VALID_ALTITUDE;
540 8392 : const bool detected_ourself = (out_state.cfg.ICAO_id != 0) && ((uint32_t)out_state.cfg.ICAO_id == vehicle.info.ICAO_address);
541 :
542 16784 : if (vehicle_loc.is_zero() ||
543 8391 : out_of_range ||
544 8391 : out_of_range_alt ||
545 8391 : detected_ourself ||
546 8391 : (vehicle.info.ICAO_address > 0x00FFFFFF) || // ICAO address is 24bits, so ignore higher values.
547 25175 : !(vehicle.info.flags & required_flags_position) ||
548 8391 : now - vehicle.last_update_ms > VEHICLE_TIMEOUT_MS) {
549 :
550 : // vehicle is out of range or invalid lat/lng. If we're tracking it, delete from list. Otherwise ignore it.
551 1 : if (is_tracked_in_list) {
552 1 : delete_vehicle(index);
553 : }
554 1 : return;
555 :
556 8391 : } else if (is_tracked_in_list) {
557 :
558 : // found, update it
559 8378 : set_vehicle(index, vehicle);
560 :
561 13 : } else if (in_state.vehicle_count < in_state.list_size_allocated) {
562 :
563 : // not found and there's room, add it to the end of the list
564 13 : set_vehicle(in_state.vehicle_count, vehicle);
565 13 : in_state.vehicle_count++;
566 :
567 : } else {
568 : // buffer is full. if new vehicle is closer than furthest, replace furthest with new
569 :
570 0 : if (my_loc_is_zero) {
571 : // nothing else to do
572 0 : in_state.furthest_vehicle_distance = 0;
573 0 : in_state.furthest_vehicle_index = 0;
574 :
575 : } else {
576 0 : if (in_state.furthest_vehicle_distance <= 0) {
577 : // ensure this is populated
578 0 : determine_furthest_aircraft();
579 : }
580 :
581 0 : if (my_loc_distance_to_vehicle < in_state.furthest_vehicle_distance) { // is closer than the furthest
582 : // replace with the furthest vehicle
583 0 : set_vehicle(in_state.furthest_vehicle_index, vehicle);
584 :
585 : // in_state.furthest_vehicle_index is now invalid because the vehicle was overwritten, need
586 : // to run determine_furthest_aircraft() to determine a new one next time
587 0 : in_state.furthest_vehicle_distance = 0;
588 0 : in_state.furthest_vehicle_index = 0;
589 : }
590 : }
591 : } // if buffer full
592 :
593 8391 : const uint16_t required_flags_avoidance =
594 : ADSB_FLAGS_VALID_COORDS |
595 : ADSB_FLAGS_VALID_ALTITUDE |
596 : ADSB_FLAGS_VALID_HEADING |
597 : ADSB_FLAGS_VALID_VELOCITY;
598 :
599 8391 : if (vehicle.info.flags & required_flags_avoidance) {
600 8391 : push_sample(vehicle); // note that set_vehicle modifies vehicle
601 : }
602 : }
603 :
604 : /*
605 : * Copy a vehicle's data into the list
606 : */
607 8391 : void AP_ADSB::set_vehicle(const uint16_t index, const adsb_vehicle_t &vehicle)
608 : {
609 8391 : if (index >= in_state.list_size_allocated) {
610 : // out of range
611 0 : return;
612 : }
613 8391 : in_state.vehicle_list[index] = vehicle;
614 :
615 : #if HAL_LOGGING_ENABLED
616 8391 : write_log(vehicle);
617 : #endif
618 : }
619 :
620 939274 : void AP_ADSB::send_adsb_vehicle(const mavlink_channel_t chan)
621 : {
622 939274 : if (!check_startup() || in_state.vehicle_count == 0) {
623 938951 : return;
624 : }
625 :
626 323 : uint32_t now = AP_HAL::millis();
627 :
628 323 : if (in_state.send_index[chan] >= in_state.vehicle_count) {
629 : // we've finished a list
630 232 : if (now - in_state.send_start_ms[chan] < 1000) {
631 : // too soon to start a new one
632 193 : return;
633 : } else {
634 : // start new list
635 39 : in_state.send_start_ms[chan] = now;
636 39 : in_state.send_index[chan] = 0;
637 : }
638 : }
639 :
640 130 : if (in_state.send_index[chan] < in_state.vehicle_count) {
641 130 : mavlink_adsb_vehicle_t vehicle = in_state.vehicle_list[in_state.send_index[chan]].info;
642 130 : in_state.send_index[chan]++;
643 :
644 130 : mavlink_msg_adsb_vehicle_send(chan,
645 : vehicle.ICAO_address,
646 : vehicle.lat,
647 : vehicle.lon,
648 130 : vehicle.altitude_type,
649 : vehicle.altitude,
650 130 : vehicle.heading,
651 130 : vehicle.hor_velocity,
652 130 : vehicle.ver_velocity,
653 : vehicle.callsign,
654 130 : vehicle.emitter_type,
655 130 : vehicle.tslc,
656 130 : vehicle.flags,
657 130 : vehicle.squawk);
658 : }
659 : }
660 :
661 : /*
662 : * handle incoming packet UAVIONIX_ADSB_OUT_CFG.
663 : * This allows a GCS to send cfg info through the autopilot to the ADSB hardware.
664 : * This is done indirectly by reading and storing the packet and then another mechanism sends it out periodically
665 : */
666 0 : void AP_ADSB::handle_out_cfg(const mavlink_uavionix_adsb_out_cfg_t &packet)
667 : {
668 0 : out_state.cfg.was_set_externally = true;
669 :
670 0 : out_state.cfg.ICAO_id = packet.ICAO;
671 0 : out_state.cfg.ICAO_id_param.set(out_state.cfg.ICAO_id_param_prev = packet.ICAO & 0x00FFFFFFFF);
672 :
673 : // May contain a non-null value at the end so accept it as-is with memcpy instead of strcpy
674 0 : memcpy(out_state.cfg.callsign, packet.callsign, sizeof(out_state.cfg.callsign));
675 :
676 0 : out_state.cfg.emitterType.set(packet.emitterType);
677 0 : out_state.cfg.lengthWidth.set(packet.aircraftSize);
678 0 : out_state.cfg.gpsOffsetLat.set(packet.gpsOffsetLat);
679 0 : out_state.cfg.gpsOffsetLon.set(packet.gpsOffsetLon);
680 0 : out_state.cfg.rfSelect.set(packet.rfSelect);
681 0 : out_state.cfg.stall_speed_cm = packet.stallSpeed;
682 :
683 : // guard against string with non-null end char
684 0 : char tmp_callsign[MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG_FIELD_CALLSIGN_LEN+1] {};
685 0 : memcpy(tmp_callsign, out_state.cfg.callsign, MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG_FIELD_CALLSIGN_LEN);
686 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ADSB: Using ICAO_id %d and Callsign %s", (int)out_state.cfg.ICAO_id, tmp_callsign);
687 :
688 : // send now
689 0 : out_state.last_config_ms = 0;
690 0 : }
691 :
692 : /*
693 : * handle incoming packet UAVIONIX_ADSB_OUT_CONTROL
694 : * allows a GCS to set the contents of the control message sent by ardupilot to the transponder
695 : */
696 0 : void AP_ADSB::handle_out_control(const mavlink_uavionix_adsb_out_control_t &packet)
697 : {
698 0 : out_state.ctrl.baroCrossChecked = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_EXTERNAL_BARO_CROSSCHECKED;
699 0 : out_state.ctrl.airGroundState = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_ON_GROUND;
700 0 : out_state.ctrl.modeAEnabled = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_MODE_A_ENABLED;
701 0 : out_state.ctrl.modeCEnabled = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_MODE_C_ENABLED;
702 0 : out_state.ctrl.modeSEnabled = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_MODE_S_ENABLED;
703 0 : out_state.ctrl.es1090TxEnabled = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_1090ES_TX_ENABLED;
704 0 : out_state.ctrl.externalBaroAltitude_mm = packet.baroAltMSL;
705 0 : out_state.ctrl.squawkCode = packet.squawk;
706 0 : out_state.ctrl.emergencyState = packet.emergencyStatus;
707 0 : memcpy(out_state.ctrl.callsign, packet.flight_id, sizeof(out_state.ctrl.callsign));
708 0 : out_state.ctrl.x_bit = packet.x_bit;
709 :
710 0 : if (packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_IDENT_BUTTON_ACTIVE) {
711 0 : IGNORE_RETURN(ident_start());
712 : }
713 0 : }
714 :
715 : /*
716 : * this is a message from the transceiver reporting it's health. Using this packet
717 : * we determine which channel is on so we don't have to send out_state to all channels
718 : */
719 0 : void AP_ADSB::handle_transceiver_report(const mavlink_channel_t chan, const mavlink_uavionix_adsb_transceiver_health_report_t &packet)
720 : {
721 0 : if (out_state.chan != chan) {
722 0 : GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "ADSB: Found transceiver on channel %d", chan);
723 : }
724 :
725 0 : out_state.chan_last_ms = AP_HAL::millis();
726 0 : out_state.chan = chan;
727 0 : out_state.status = (UAVIONIX_ADSB_RF_HEALTH)packet.rfHealth;
728 0 : }
729 :
730 : /*
731 : * send a periodic report of the ADSB out status
732 : */
733 0 : void AP_ADSB::send_adsb_out_status(const mavlink_channel_t chan) const
734 : {
735 0 : for (uint8_t i=0; i < ADSB_MAX_INSTANCES; i++) {
736 0 : if (_type[i] == (int8_t)(AP_ADSB::Type::uAvionix_UCP) || _type[i] == (int8_t)(AP_ADSB::Type::Sagetech_MXS)) {
737 0 : mavlink_msg_uavionix_adsb_out_status_send_struct(chan, &out_state.tx_status);
738 0 : return;
739 : }
740 : }
741 : }
742 :
743 : /*
744 : @brief Generates pseudorandom ICAO from gps time, lat, and lon.
745 : Reference: DO282B, 2.2.4.5.1.3.2
746 : */
747 0 : uint32_t AP_ADSB::genICAO(const Location &loc) const
748 : {
749 : // gps_time is used as a pseudo-random number instead of seconds since UTC midnight
750 : // TODO: use UTC time instead of GPS time
751 0 : const AP_ADSB::Loc &gps { _my_loc };
752 0 : const uint64_t gps_time = gps.time_epoch_usec();
753 :
754 0 : uint32_t timeSum = 0;
755 0 : const uint32_t M3 = 4096 * (loc.lat & 0x00000FFF) + (loc.lng & 0x00000FFF);
756 :
757 0 : for (uint8_t i=0; i<24; i++) {
758 0 : timeSum += (((gps_time & 0x00FFFFFF)>> i) & 0x00000001);
759 : }
760 0 : return( (timeSum ^ M3) & 0x00FFFFFF);
761 : }
762 :
763 : // assign a string to out_state.cfg.callsign but ensure it's null terminated
764 0 : void AP_ADSB::set_callsign(const char* str, const bool append_icao)
765 : {
766 0 : bool zero_char_pad = false;
767 :
768 : // clean slate
769 0 : memset(out_state.cfg.callsign, 0, sizeof(out_state.cfg.callsign));
770 :
771 : // copy str to cfg.callsign but we can't use strncpy because we need
772 : // to restrict values to only 'A' - 'Z' and '0' - '9' and pad
773 0 : for (uint8_t i=0; i<sizeof(out_state.cfg.callsign)-1; i++) {
774 0 : if (!str[i] || zero_char_pad) {
775 : // finish early. Either pad the rest with zero char or null terminate and call it a day
776 0 : if ((append_icao && i<4) || zero_char_pad) {
777 0 : out_state.cfg.callsign[i] = '0';
778 0 : zero_char_pad = true;
779 : } else {
780 : // already null terminated via memset so just stop
781 : break;
782 : }
783 :
784 0 : } else if (('A' <= str[i] && str[i] <= 'Z') ||
785 0 : ('0' <= str[i] && str[i] <= '9')) {
786 : // valid as-is
787 : // spaces are also allowed but are handled in the last else
788 0 : out_state.cfg.callsign[i] = str[i];
789 :
790 0 : } else if ('a' <= str[i] && str[i] <= 'z') {
791 : // toupper()
792 0 : out_state.cfg.callsign[i] = str[i] - ('a' - 'A');
793 :
794 0 : } else if (i == 0) {
795 : // invalid, pad to char zero because first index can't be space
796 0 : out_state.cfg.callsign[i] = '0';
797 :
798 : } else {
799 : // invalid, pad with space
800 0 : out_state.cfg.callsign[i] = ' ';
801 : }
802 : } // for i
803 :
804 0 : if (append_icao) {
805 0 : hal.util->snprintf(&out_state.cfg.callsign[4], 5, "%04X", unsigned(out_state.cfg.ICAO_id % 0x10000));
806 : }
807 0 : }
808 :
809 :
810 8391 : void AP_ADSB::push_sample(const adsb_vehicle_t &vehicle)
811 : {
812 8391 : _samples.push(vehicle);
813 8391 : }
814 :
815 2637 : bool AP_ADSB::next_sample(adsb_vehicle_t &vehicle)
816 : {
817 2637 : return _samples.pop(vehicle);
818 : }
819 :
820 15 : void AP_ADSB::handle_message(const mavlink_channel_t chan, const mavlink_message_t &msg)
821 : {
822 15 : switch (msg.msgid) {
823 15 : case MAVLINK_MSG_ID_ADSB_VEHICLE: {
824 15 : adsb_vehicle_t vehicle {};
825 15 : mavlink_msg_adsb_vehicle_decode(&msg, &vehicle.info);
826 15 : vehicle.last_update_ms = AP_HAL::millis() - uint32_t(vehicle.info.tslc * 1000U);
827 15 : handle_adsb_vehicle(vehicle);
828 15 : break;
829 : }
830 :
831 0 : case MAVLINK_MSG_ID_UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT: {
832 0 : mavlink_uavionix_adsb_transceiver_health_report_t packet {};
833 0 : mavlink_msg_uavionix_adsb_transceiver_health_report_decode(&msg, &packet);
834 0 : handle_transceiver_report(chan, packet);
835 0 : break;
836 : }
837 :
838 0 : case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG: {
839 0 : mavlink_uavionix_adsb_out_cfg_t packet {};
840 0 : mavlink_msg_uavionix_adsb_out_cfg_decode(&msg, &packet);
841 0 : handle_out_cfg(packet);
842 0 : break;
843 : }
844 :
845 0 : case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC:
846 : // unhandled, this is an outbound packet only
847 0 : break;
848 :
849 0 : case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CONTROL: {
850 0 : mavlink_uavionix_adsb_out_control_t packet {};
851 0 : mavlink_msg_uavionix_adsb_out_control_decode(&msg, &packet);
852 0 : handle_out_control(packet);
853 0 : break;
854 : }
855 : }
856 :
857 15 : }
858 :
859 : // If that ICAO is found in the database then return true with a fully populated vehicle
860 8383 : bool AP_ADSB::get_vehicle_by_ICAO(const uint32_t icao, adsb_vehicle_t &vehicle) const
861 : {
862 : adsb_vehicle_t temp_vehicle;
863 8383 : temp_vehicle.info.ICAO_address = icao;
864 :
865 : uint16_t i;
866 8383 : if (find_index(temp_vehicle, &i)) {
867 : // vehicle is tracked in list.
868 : // we must memcpy it because the database may reorganize itself and we don't
869 : // want the reference to magically start pointing at a different vehicle
870 8377 : memcpy(&vehicle, &in_state.vehicle_list[i], sizeof(adsb_vehicle_t));
871 8377 : return true;
872 : }
873 6 : return false;
874 : }
875 :
876 : #if HAL_LOGGING_ENABLED
877 : /*
878 : * Write vehicle to log
879 : */
880 8391 : void AP_ADSB::write_log(const adsb_vehicle_t &vehicle) const
881 : {
882 8391 : switch ((Logging)_log) {
883 8391 : case Logging::SPECIAL_ONLY:
884 8391 : if (!is_special_vehicle(vehicle.info.ICAO_address)) {
885 8391 : return;
886 : }
887 0 : break;
888 :
889 0 : case Logging::ALL:
890 0 : break;
891 :
892 0 : case Logging::NONE:
893 : default:
894 0 : return;
895 : }
896 :
897 0 : struct log_ADSB pkt = {
898 : LOG_PACKET_HEADER_INIT(LOG_ADSB_MSG),
899 0 : time_us : AP_HAL::micros64(),
900 0 : ICAO_address : vehicle.info.ICAO_address,
901 0 : lat : vehicle.info.lat,
902 0 : lng : vehicle.info.lon,
903 0 : alt : vehicle.info.altitude,
904 0 : heading : vehicle.info.heading,
905 0 : hor_velocity : vehicle.info.hor_velocity,
906 0 : ver_velocity : vehicle.info.ver_velocity,
907 0 : squawk : vehicle.info.squawk,
908 0 : };
909 0 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
910 : }
911 : #endif // HAL_LOGGING_ENABLED
912 :
913 : /**
914 : * @brief Convert base 8 or 16 to decimal. Used to convert an octal/hexadecimal value stored on a GCS as a string field in different format, but then transmitted over mavlink as a float which is always a decimal.
915 : * baseIn: base of input number
916 : * inputNumber: value currently in base "baseIn" to be converted to base "baseOut"
917 : *
918 : * Example: convert ADSB squawk octal "1200" stored in memory as 0x0280 to 0x04B0
919 : * uint16_t squawk_decimal = convertMathBase(8, squawk_octal);
920 : */
921 11 : uint32_t AP_ADSB::convert_base_to_decimal(const uint8_t baseIn, uint32_t inputNumber)
922 : {
923 : // Our only sensible input bases are 16 and 8
924 11 : if (baseIn != 8 && baseIn != 16) {
925 0 : return inputNumber;
926 : }
927 11 : uint32_t outputNumber = 0;
928 44 : for (uint8_t i=0; i < 10; i++) {
929 44 : outputNumber += (inputNumber % 10) * powf(baseIn, i);
930 44 : inputNumber /= 10;
931 44 : if (inputNumber == 0) break;
932 : }
933 11 : return outputNumber;
934 : }
935 :
936 : // Trigger a Mode 3/A transponder IDENT. This should only be done when requested to do so by an Air Traffic Controller.
937 : // See wikipedia for IDENT explanation https://en.wikipedia.org/wiki/Transponder_(aeronautics)
938 0 : bool AP_ADSB::ident_start()
939 : {
940 0 : if (!healthy()) {
941 0 : return false;
942 : }
943 0 : out_state.ctrl.identActive = true;
944 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,"ADSB: IDENT!");
945 0 : return true;
946 : }
947 :
948 : // methods for embedded class Location
949 9 : bool AP_ADSB::Loc::speed_accuracy(float &sacc) const
950 : {
951 9 : if (!horizontal_vel_accuracy_is_valid) {
952 7 : return false;
953 : }
954 2 : sacc = horizontal_vel_accuracy;
955 2 : return true;
956 : }
957 :
958 9 : bool AP_ADSB::Loc::horizontal_accuracy(float &hacc) const
959 : {
960 9 : if (!horizontal_pos_accuracy_is_valid) {
961 7 : return false;
962 : }
963 2 : hacc = horizontal_pos_accuracy;
964 2 : return true;
965 : }
966 :
967 9 : bool AP_ADSB::Loc::vertical_accuracy(float &vacc) const
968 : {
969 9 : if (!vertical_pos_accuracy_is_valid) {
970 7 : return false;
971 : }
972 2 : vacc = vertical_pos_accuracy;
973 2 : return true;
974 : }
975 :
976 3 : uint8_t AP_ADSB::convert_maxknots_to_enum(const float maxAircraftSpeed_knots)
977 : {
978 3 : if (maxAircraftSpeed_knots <= 0) {
979 : // not set or unknown. no bits set
980 1 : return 0;
981 2 : } else if (maxAircraftSpeed_knots <= 75) {
982 2 : return 1;
983 0 : } else if (maxAircraftSpeed_knots <= 150) {
984 0 : return 2;
985 0 : } else if (maxAircraftSpeed_knots <= 300) {
986 0 : return 3;
987 0 : } else if (maxAircraftSpeed_knots <= 600) {
988 0 : return 4;
989 0 : } else if (maxAircraftSpeed_knots <= 1200) {
990 0 : return 5;
991 : } else {
992 0 : return 6;
993 : }
994 : }
995 :
996 66592 : AP_ADSB *AP::ADSB()
997 : {
998 66592 : return AP_ADSB::get_singleton();
999 : }
1000 : #endif // HAL_ADSB_ENABLED
|
