Line data Source code
1 : /*
2 : * This file is free software: you can redistribute it and/or modify it
3 : * under the terms of the GNU General Public License as published by the
4 : * Free Software Foundation, either version 3 of the License, or
5 : * (at your option) any later version.
6 : *
7 : * This file is distributed in the hope that it will be useful, but
8 : * WITHOUT ANY WARRANTY; without even the implied warranty of
9 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
10 : * See the GNU General Public License for more details.
11 : *
12 : * You should have received a copy of the GNU General Public License along
13 : * with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : *
17 : * original code by:
18 : * BlueMark Innovations BV, Roel Schiphorst
19 : * Contributors: Tom Pittenger, Josh Henderson, Andrew Tridgell
20 : * Parts of this code are based on/copied from the Open Drone ID project https://github.com/opendroneid/opendroneid-core-c
21 : *
22 : * The code has been tested with the BlueMark DroneBeacon MAVLink transponder running this command in the ArduPlane folder:
23 : * sim_vehicle.py --console --map -A --serial1=uart:/dev/ttyUSB1:9600
24 : * (and a DroneBeacon MAVLink transponder connected to ttyUSB1)
25 : *
26 : * See https://github.com/ArduPilot/ArduRemoteID for an open implementation of a transmitter module on serial
27 : * and DroneCAN
28 : */
29 :
30 : #include "AP_OpenDroneID_config.h"
31 :
32 : #if AP_OPENDRONEID_ENABLED
33 :
34 : #include "AP_OpenDroneID.h"
35 : #include <AP_HAL/AP_HAL.h>
36 : #include <GCS_MAVLink/GCS.h>
37 : #include <AP_GPS/AP_GPS.h>
38 : #include <AP_Baro/AP_Baro.h>
39 : #include <AP_AHRS/AP_AHRS.h>
40 : #include <AP_Parachute/AP_Parachute.h>
41 : #include <AP_Vehicle/AP_Vehicle.h>
42 : #include <AP_DroneCAN/AP_DroneCAN.h>
43 : #include <stdio.h>
44 : #include <GCS_MAVLink/GCS.h>
45 :
46 : extern const AP_HAL::HAL &hal;
47 :
48 : const AP_Param::GroupInfo AP_OpenDroneID::var_info[] = {
49 :
50 : // @Param: ENABLE
51 : // @DisplayName: Enable ODID subsystem
52 : // @Description: Enable ODID subsystem
53 : // @Values: 0:Disabled,1:Enabled
54 : AP_GROUPINFO_FLAGS("ENABLE", 1, AP_OpenDroneID, _enable, 0, AP_PARAM_FLAG_ENABLE),
55 :
56 : // @Param: MAVPORT
57 : // @DisplayName: MAVLink serial port
58 : // @Description: Serial port number to send OpenDroneID MAVLink messages to. Can be -1 if using DroneCAN.
59 : // @Values: -1:Disabled,0:Serial0,1:Serial1,2:Serial2,3:Serial3,4:Serial4,5:Serial5,6:Serial6
60 : AP_GROUPINFO("MAVPORT", 2, AP_OpenDroneID, _mav_port, -1),
61 :
62 : // @Param: CANDRIVER
63 : // @DisplayName: DroneCAN driver number
64 : // @Description: DroneCAN driver index, 0 to disable DroneCAN
65 : // @Values: 0:Disabled,1:Driver1,2:Driver2
66 : AP_GROUPINFO("CANDRIVER", 3, AP_OpenDroneID, _can_driver, 0),
67 :
68 : // @Param: OPTIONS
69 : // @DisplayName: OpenDroneID options
70 : // @Description: Options for OpenDroneID subsystem
71 : // @Bitmask: 0:EnforcePreArmChecks, 1:AllowNonGPSPosition, 2:LockUASIDOnFirstBasicIDRx
72 : AP_GROUPINFO("OPTIONS", 4, AP_OpenDroneID, _options, 0),
73 :
74 : // @Param: BARO_ACC
75 : // @DisplayName: Barometer vertical accuraacy
76 : // @Description: Barometer Vertical Accuracy when installed in the vehicle. Note this is dependent upon installation conditions and thus disabled by default
77 : // @Units: m
78 : // @User: Advanced
79 : AP_GROUPINFO("BARO_ACC", 5, AP_OpenDroneID, _baro_accuracy, -1.0),
80 :
81 : AP_GROUPEND
82 : };
83 :
84 : #if defined(OPENDRONEID_UA_TYPE)
85 : // ensure the type is within the allowed range
86 : #if OPENDRONEID_UA_TYPE < 0 || OPENDRONEID_UA_TYPE > 15
87 : #error "OPENDRONEID_UA_TYPE must be between 0 and 15"
88 : #endif
89 : #endif
90 :
91 : // constructor
92 2096 : AP_OpenDroneID::AP_OpenDroneID()
93 : {
94 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
95 2096 : if (_singleton != nullptr) {
96 0 : AP_HAL::panic("OpenDroneID must be singleton");
97 : }
98 : #endif
99 2096 : _singleton = this;
100 2096 : AP_Param::setup_object_defaults(this, var_info);
101 2096 : }
102 :
103 2086 : void AP_OpenDroneID::init()
104 : {
105 2086 : if (_enable == 0) {
106 2086 : return;
107 : }
108 :
109 0 : load_UAS_ID_from_persistent_memory();
110 0 : _chan = mavlink_channel_t(gcs().get_channel_from_port_number(_mav_port));
111 0 : _initialised = true;
112 : }
113 :
114 0 : void AP_OpenDroneID::load_UAS_ID_from_persistent_memory()
115 : {
116 0 : id_len = sizeof(id_str);
117 0 : size_t id_type_len = sizeof(id_type);
118 0 : size_t ua_type_len = sizeof(ua_type);
119 0 : if (hal.util->get_persistent_param_by_name("DID_UAS_ID", id_str, id_len) &&
120 0 : hal.util->get_persistent_param_by_name("DID_UAS_ID_TYPE", id_type, id_type_len) &&
121 0 : hal.util->get_persistent_param_by_name("DID_UA_TYPE", ua_type, ua_type_len)) {
122 0 : if (id_len && id_type_len && ua_type_len) {
123 0 : _options.set_and_save(_options.get() & ~LockUASIDOnFirstBasicIDRx);
124 0 : _options.notify();
125 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "OpenDroneID: Locked UAS_ID: %s", id_str);
126 : }
127 : } else {
128 0 : id_len = 0;
129 : }
130 0 : }
131 :
132 0 : void AP_OpenDroneID::set_basic_id() {
133 0 : if (pkt_basic_id.id_type != MAV_ODID_ID_TYPE_NONE) {
134 0 : return;
135 : }
136 0 : if (id_len > 0) {
137 : // prepare basic id pkt
138 0 : uint8_t val = gcs().sysid_this_mav();
139 0 : pkt_basic_id.target_system = val;
140 0 : pkt_basic_id.target_component = MAV_COMP_ID_ODID_TXRX_1;
141 0 : pkt_basic_id.id_type = atoi(id_type);
142 0 : pkt_basic_id.ua_type = atoi(ua_type);
143 : char buffer[21];
144 0 : snprintf(buffer, sizeof(buffer), "%s", id_str);
145 0 : memcpy(pkt_basic_id.uas_id, buffer, sizeof(pkt_basic_id.uas_id));
146 : }
147 : }
148 :
149 0 : void AP_OpenDroneID::get_persistent_params(ExpandingString &str) const
150 : {
151 0 : if ((pkt_basic_id.id_type == MAV_ODID_ID_TYPE_SERIAL_NUMBER)
152 0 : && (_options & LockUASIDOnFirstBasicIDRx)
153 0 : && id_len == 0) {
154 : static constexpr size_t uas_id_size = sizeof(pkt_basic_id.uas_id);
155 : char buffer[uas_id_size+1];
156 0 : memcpy(buffer, pkt_basic_id.uas_id, uas_id_size);
157 0 : buffer[uas_id_size] = '\0'; // make null terminated
158 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "OpenDroneID: ID is locked as %s", buffer);
159 0 : str.printf("DID_UAS_ID=%s\nDID_UAS_ID_TYPE=%u\nDID_UA_TYPE=%u\n", buffer, pkt_basic_id.id_type, pkt_basic_id.ua_type);
160 : }
161 0 : }
162 :
163 : // Perform the pre-arm checks and prevent arming if they are not satisifed
164 : // Except in the case of an in-flight reboot
165 140684 : bool AP_OpenDroneID::pre_arm_check(char* failmsg, uint8_t failmsg_len)
166 : {
167 140684 : WITH_SEMAPHORE(_sem);
168 :
169 140684 : if (!option_enabled(Options::EnforcePreArmChecks)) {
170 140684 : return true;
171 : }
172 :
173 0 : if(_enable == 0) {
174 0 : strncpy(failmsg, "DID_ENABLE must be 1", failmsg_len);
175 0 : return false;
176 : }
177 :
178 0 : if (pkt_basic_id.id_type == MAV_ODID_ID_TYPE_NONE) {
179 0 : strncpy(failmsg, "UA_TYPE required in BasicID", failmsg_len);
180 0 : return false;
181 : }
182 :
183 0 : if (pkt_system.operator_latitude == 0 && pkt_system.operator_longitude == 0) {
184 0 : strncpy(failmsg, "operator location must be set", failmsg_len);
185 0 : return false;
186 : }
187 :
188 0 : const uint32_t max_age_ms = 3000;
189 0 : const uint32_t now_ms = AP_HAL::millis();
190 :
191 0 : if (last_arm_status_ms == 0 || now_ms - last_arm_status_ms > max_age_ms) {
192 0 : strncpy(failmsg, "ARM_STATUS not available", failmsg_len);
193 0 : return false;
194 : }
195 :
196 0 : if (last_system_ms == 0 ||
197 0 : (now_ms - last_system_ms > max_age_ms &&
198 0 : (now_ms - last_system_update_ms > max_age_ms))) {
199 0 : strncpy(failmsg, "SYSTEM not available", failmsg_len);
200 0 : return false;
201 : }
202 :
203 0 : if (arm_status.status != MAV_ODID_ARM_STATUS_GOOD_TO_ARM) {
204 0 : strncpy(failmsg, arm_status.error, failmsg_len);
205 0 : return false;
206 : }
207 :
208 0 : return true;
209 140684 : }
210 :
211 1504089 : void AP_OpenDroneID::update()
212 : {
213 1504089 : if (_enable == 0) {
214 1504089 : return;
215 : }
216 :
217 0 : if ((pkt_basic_id.id_type == MAV_ODID_ID_TYPE_SERIAL_NUMBER)
218 0 : && (_options & LockUASIDOnFirstBasicIDRx)
219 0 : && id_len == 0
220 0 : && !bootloader_flashed) {
221 0 : hal.util->flash_bootloader();
222 : // reset the basic id on next set_basic_id call
223 0 : pkt_basic_id.id_type = MAV_ODID_ID_TYPE_NONE;
224 0 : bootloader_flashed = true;
225 : }
226 :
227 0 : set_basic_id();
228 :
229 0 : const bool armed = hal.util->get_soft_armed();
230 0 : if (armed && !_was_armed) {
231 : // use arm location as takeoff location
232 : // if the AHRS can't give us a result it will pass GPS
233 : // location in _takeoff_location while returning false:
234 0 : UNUSED_RESULT(AP::ahrs().get_location(_takeoff_location));
235 : }
236 0 : _was_armed = armed;
237 :
238 0 : send_dynamic_out();
239 0 : send_static_out();
240 : #if HAL_ENABLE_DRONECAN_DRIVERS
241 0 : uint8_t can_num_drivers = AP::can().get_num_drivers();
242 0 : for (uint8_t i = 0; i < can_num_drivers; i++) {
243 0 : AP_DroneCAN *dronecan = AP_DroneCAN::get_dronecan(i);
244 0 : if (dronecan == nullptr) {
245 0 : continue;
246 : }
247 0 : if (dronecan->get_driver_index()+1 != _can_driver) {
248 0 : continue;
249 : }
250 : // send messages
251 0 : dronecan_send(dronecan);
252 : }
253 : #endif
254 : }
255 :
256 : // local payload space check which treats invalid channel as having space
257 : // needed to populate the message structures for the DroneCAN backend
258 : #define ODID_HAVE_PAYLOAD_SPACE(id) (_chan == MAV_CHAN_INVALID || HAVE_PAYLOAD_SPACE(_chan, id))
259 :
260 0 : void AP_OpenDroneID::send_dynamic_out()
261 : {
262 0 : const uint32_t now = AP_HAL::millis();
263 0 : if (now - _last_send_location_ms >= _mavlink_dynamic_period_ms &&
264 0 : ODID_HAVE_PAYLOAD_SPACE(OPEN_DRONE_ID_LOCATION)) {
265 0 : _last_send_location_ms = now;
266 0 : send_location_message();
267 : }
268 :
269 : // operator location needs to be sent at the same rate as location for FAA compliance
270 0 : if (now - _last_send_system_update_ms >= _mavlink_dynamic_period_ms &&
271 0 : ODID_HAVE_PAYLOAD_SPACE(OPEN_DRONE_ID_SYSTEM_UPDATE)) {
272 0 : _last_send_system_update_ms = now;
273 0 : send_system_update_message();
274 : }
275 0 : }
276 :
277 0 : void AP_OpenDroneID::send_static_out()
278 : {
279 0 : const uint32_t now_ms = AP_HAL::millis();
280 :
281 : // we need to notify user if we lost the transmitter
282 0 : if (now_ms - last_arm_status_ms > 5000) {
283 0 : if (now_ms - last_lost_tx_ms > 5000) {
284 0 : last_lost_tx_ms = now_ms;
285 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ODID: lost transmitter");
286 : }
287 0 : } else if (last_lost_tx_ms != 0) {
288 : // we're OK again
289 0 : last_lost_tx_ms = 0;
290 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ODID: transmitter OK");
291 : }
292 :
293 : // we need to notify user if we lost system msg with operator location
294 0 : if (now_ms - last_system_ms > 5000 && now_ms - last_lost_operator_msg_ms > 5000) {
295 0 : last_lost_operator_msg_ms = now_ms;
296 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ODID: lost operator location");
297 : }
298 :
299 0 : const uint32_t msg_spacing_ms = _mavlink_static_period_ms / 4;
300 0 : if (now_ms - last_msg_send_ms >= msg_spacing_ms) {
301 : // allow update of channel during setup, this makes it easy to debug with a GCS
302 0 : _chan = mavlink_channel_t(gcs().get_channel_from_port_number(_mav_port));
303 0 : bool sent_ok = false;
304 0 : switch (next_msg_to_send) {
305 0 : case NEXT_MSG_BASIC_ID:
306 0 : if (ODID_HAVE_PAYLOAD_SPACE(OPEN_DRONE_ID_BASIC_ID)) {
307 0 : send_basic_id_message();
308 0 : sent_ok = true;
309 : }
310 0 : break;
311 0 : case NEXT_MSG_SYSTEM:
312 0 : if (ODID_HAVE_PAYLOAD_SPACE(OPEN_DRONE_ID_SYSTEM)) {
313 0 : send_system_message();
314 0 : sent_ok = true;
315 : }
316 0 : break;
317 0 : case NEXT_MSG_SELF_ID:
318 0 : if (ODID_HAVE_PAYLOAD_SPACE(OPEN_DRONE_ID_SELF_ID)) {
319 0 : send_self_id_message();
320 0 : sent_ok = true;
321 : }
322 0 : break;
323 0 : case NEXT_MSG_OPERATOR_ID:
324 0 : if (ODID_HAVE_PAYLOAD_SPACE(OPEN_DRONE_ID_OPERATOR_ID)) {
325 0 : send_operator_id_message();
326 0 : sent_ok = true;
327 : }
328 0 : break;
329 0 : case NEXT_MSG_ENUM_END:
330 0 : break;
331 : }
332 0 : if (sent_ok) {
333 0 : last_msg_send_ms = now_ms;
334 0 : next_msg_to_send = next_msg((uint8_t(next_msg_to_send) + 1) % uint8_t(NEXT_MSG_ENUM_END));
335 : }
336 : }
337 0 : }
338 :
339 : // The send_location_message
340 : // all open_drone_id send functions use data stored in the open drone id class.
341 : // This location send function is an exception. It uses live location data from the ArduPilot system.
342 0 : void AP_OpenDroneID::send_location_message()
343 : {
344 0 : auto &ahrs = AP::ahrs();
345 0 : const auto &barometer = AP::baro();
346 0 : const auto &gps = AP::gps();
347 :
348 0 : const AP_GPS_FixType gps_status = gps.status();
349 0 : const bool got_bad_gps_fix = (gps_status < AP_GPS_FixType::FIX_3D);
350 0 : const bool armed = hal.util->get_soft_armed();
351 :
352 0 : Location current_location;
353 0 : if (!ahrs.get_location(current_location)) {
354 0 : return;
355 : }
356 0 : uint8_t uav_status = hal.util->get_soft_armed()? MAV_ODID_STATUS_AIRBORNE : MAV_ODID_STATUS_GROUND;
357 : #if HAL_PARACHUTE_ENABLED
358 : // set emergency status if chute is released
359 0 : const auto *parachute = AP::parachute();
360 0 : if (parachute != nullptr && parachute->released()) {
361 0 : uav_status = MAV_ODID_STATUS_EMERGENCY;
362 : }
363 : #endif
364 0 : if (AP::vehicle()->is_crashed()) {
365 : // if in crashed state also declare an emergency
366 0 : uav_status = MAV_ODID_STATUS_EMERGENCY;
367 : }
368 :
369 : // if we are armed with no GPS fix and we haven't specifically
370 : // allowed for non-GPS operation then declare an emergency
371 0 : if (got_bad_gps_fix && armed && !option_enabled(Options::AllowNonGPSPosition)) {
372 0 : uav_status = MAV_ODID_STATUS_EMERGENCY;
373 : }
374 :
375 : // if we are disarmed and falling at over 3m/s then declare an
376 : // emergency. This covers cases such as deliberate crash with
377 : // advanced failsafe and an unintended reboot or in-flight disarm
378 0 : if (!got_bad_gps_fix && !armed && gps.velocity().z > 3.0) {
379 0 : uav_status = MAV_ODID_STATUS_EMERGENCY;
380 : }
381 :
382 : // if we have watchdogged while armed then declare an emergency
383 0 : if (hal.util->was_watchdog_armed()) {
384 0 : uav_status = MAV_ODID_STATUS_EMERGENCY;
385 : }
386 :
387 0 : float direction = ODID_INV_DIR;
388 0 : if (!got_bad_gps_fix) {
389 0 : direction = wrap_360(degrees(ahrs.groundspeed_vector().angle())); // heading (degrees)
390 : }
391 :
392 0 : const float speed_horizontal = create_speed_horizontal(ahrs.groundspeed());
393 :
394 0 : Vector3f velNED;
395 0 : UNUSED_RESULT(ahrs.get_velocity_NED(velNED));
396 0 : const float climb_rate = create_speed_vertical(-velNED.z); //make sure climb_rate is within Remote ID limit
397 :
398 0 : int32_t latitude = 0;
399 0 : int32_t longitude = 0;
400 0 : if (current_location.check_latlng()) { //set location if they are valid
401 0 : latitude = current_location.lat;
402 0 : longitude = current_location.lng;
403 : }
404 :
405 : // altitude referenced against 1013.2mb
406 0 : const float base_press_mbar = 1013.2;
407 0 : const float altitude_barometric = create_altitude(barometer.get_altitude_difference(base_press_mbar*100, barometer.get_pressure()));
408 :
409 0 : float altitude_geodetic = -1000;
410 : int32_t alt_amsl_cm;
411 : float undulation;
412 0 : if (current_location.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_amsl_cm)) {
413 0 : altitude_geodetic = alt_amsl_cm * 0.01;
414 : }
415 0 : if (gps.get_undulation(undulation)) {
416 0 : altitude_geodetic -= undulation;
417 : }
418 :
419 : // Compute the current height above the takeoff location
420 0 : float height_above_takeoff = 0; // height above takeoff (meters)
421 0 : if (hal.util->get_soft_armed()) {
422 : int32_t curr_alt_asml_cm;
423 : int32_t takeoff_alt_asml_cm;
424 0 : if (current_location.get_alt_cm(Location::AltFrame::ABSOLUTE, curr_alt_asml_cm) &&
425 0 : _takeoff_location.get_alt_cm(Location::AltFrame::ABSOLUTE, takeoff_alt_asml_cm)) {
426 0 : height_above_takeoff = (curr_alt_asml_cm - takeoff_alt_asml_cm) * 0.01;
427 : }
428 : }
429 :
430 : // Accuracy
431 :
432 : // If we have GPS 3D lock we presume that the accuracies of the system will track the GPS's reported accuracy
433 0 : MAV_ODID_HOR_ACC horizontal_accuracy_mav = MAV_ODID_HOR_ACC_UNKNOWN;
434 0 : MAV_ODID_VER_ACC vertical_accuracy_mav = MAV_ODID_VER_ACC_UNKNOWN;
435 0 : MAV_ODID_SPEED_ACC speed_accuracy_mav = MAV_ODID_SPEED_ACC_UNKNOWN;
436 0 : MAV_ODID_TIME_ACC timestamp_accuracy_mav = MAV_ODID_TIME_ACC_UNKNOWN;
437 :
438 : float horizontal_accuracy;
439 0 : if (gps.horizontal_accuracy(horizontal_accuracy)) {
440 0 : horizontal_accuracy_mav = create_enum_horizontal_accuracy(horizontal_accuracy);
441 : }
442 :
443 : float vertical_accuracy;
444 0 : if (gps.vertical_accuracy(vertical_accuracy)) {
445 0 : vertical_accuracy_mav = create_enum_vertical_accuracy(vertical_accuracy);
446 : }
447 :
448 : float speed_accuracy;
449 0 : if (gps.speed_accuracy(speed_accuracy)) {
450 0 : speed_accuracy_mav = create_enum_speed_accuracy(speed_accuracy);
451 : }
452 :
453 : // if we have ever had GPS lock then we will have better than 1s
454 : // accuracy, as we use system timer to propogate time
455 0 : timestamp_accuracy_mav = create_enum_timestamp_accuracy(1.0);
456 :
457 : // Barometer altitude accuraacy will be highly dependent on the airframe and installation of the barometer in use
458 : // thus ArduPilot cannot reasonably fill this in.
459 : // Instead allow a manufacturer to use a parameter to fill this in
460 0 : uint8_t barometer_accuracy = MAV_ODID_VER_ACC_UNKNOWN; //ahrs class does not provide accuracy readings
461 0 : if (!is_equal(_baro_accuracy.get(), -1.0f)) {
462 0 : barometer_accuracy = create_enum_vertical_accuracy(_baro_accuracy);
463 : }
464 :
465 : // Timestamp here is the number of seconds after into the current hour referenced to UTC time (up to one hour)
466 :
467 : // FIX we need to only set this if w have a GPS lock is 2D good enough for that?
468 0 : float timestamp = ODID_INV_TIMESTAMP;
469 0 : if (!got_bad_gps_fix) {
470 0 : uint32_t time_week_ms = gps.time_week_ms();
471 0 : timestamp = float(time_week_ms % (3600 * 1000)) * 0.001;
472 0 : timestamp = create_location_timestamp(timestamp); //make sure timestamp is within Remote ID limit
473 : }
474 :
475 :
476 : {
477 0 : WITH_SEMAPHORE(_sem);
478 : // take semaphore so CAN gets a consistent packet
479 0 : pkt_location = mavlink_open_drone_id_location_t{
480 : latitude : latitude,
481 : longitude : longitude,
482 : altitude_barometric : altitude_barometric,
483 : altitude_geodetic : altitude_geodetic,
484 : height : height_above_takeoff,
485 : timestamp : timestamp,
486 0 : direction : uint16_t(direction * 100.0), // Heading (centi-degrees)
487 0 : speed_horizontal : uint16_t(speed_horizontal * 100.0), // Ground speed (cm/s)
488 0 : speed_vertical : int16_t(climb_rate * 100.0), // Climb rate (cm/s)
489 : target_system : 0,
490 : target_component : 0,
491 : id_or_mac : {},
492 : status : uint8_t(uav_status),
493 : height_reference : MAV_ODID_HEIGHT_REF_OVER_TAKEOFF, // height reference enum: Above takeoff location or above ground
494 : horizontal_accuracy : uint8_t(horizontal_accuracy_mav),
495 : vertical_accuracy : uint8_t(vertical_accuracy_mav),
496 : barometer_accuracy : barometer_accuracy,
497 : speed_accuracy : uint8_t(speed_accuracy_mav),
498 : timestamp_accuracy : uint8_t(timestamp_accuracy_mav)
499 : };
500 0 : need_send_location = dronecan_send_all;
501 0 : }
502 :
503 0 : if (_chan != MAV_CHAN_INVALID) {
504 0 : mavlink_msg_open_drone_id_location_send_struct(_chan, &pkt_location);
505 : }
506 : }
507 :
508 0 : void AP_OpenDroneID::send_basic_id_message()
509 : {
510 : // note that packet is filled in by the GCS
511 0 : need_send_basic_id |= dronecan_send_all;
512 0 : if (_chan != MAV_CHAN_INVALID) {
513 0 : mavlink_msg_open_drone_id_basic_id_send_struct(_chan, &pkt_basic_id);
514 : }
515 0 : }
516 :
517 0 : void AP_OpenDroneID::send_system_message()
518 : {
519 : // note that packet is filled in by the GCS
520 0 : need_send_system |= dronecan_send_all;
521 0 : if (_chan != MAV_CHAN_INVALID) {
522 0 : mavlink_msg_open_drone_id_system_send_struct(_chan, &pkt_system);
523 : }
524 0 : }
525 :
526 0 : void AP_OpenDroneID::send_self_id_message()
527 : {
528 0 : need_send_self_id |= dronecan_send_all;
529 0 : if (_chan != MAV_CHAN_INVALID) {
530 0 : mavlink_msg_open_drone_id_self_id_send_struct(_chan, &pkt_self_id);
531 : }
532 0 : }
533 :
534 0 : void AP_OpenDroneID::send_system_update_message()
535 : {
536 0 : need_send_system |= dronecan_send_all;
537 : // note that packet is filled in by the GCS
538 0 : if (_chan != MAV_CHAN_INVALID) {
539 : const auto pkt_system_update = mavlink_open_drone_id_system_update_t {
540 0 : operator_latitude : pkt_system.operator_latitude,
541 0 : operator_longitude : pkt_system.operator_longitude,
542 0 : operator_altitude_geo : pkt_system.operator_altitude_geo,
543 0 : timestamp : pkt_system.timestamp,
544 0 : target_system : pkt_system.target_system,
545 0 : target_component : pkt_system.target_component,
546 0 : };
547 0 : mavlink_msg_open_drone_id_system_update_send_struct(_chan, &pkt_system_update);
548 : }
549 0 : }
550 :
551 0 : void AP_OpenDroneID::send_operator_id_message()
552 : {
553 0 : need_send_operator_id |= dronecan_send_all;
554 : // note that packet is filled in by the GCS
555 0 : if (_chan != MAV_CHAN_INVALID) {
556 0 : mavlink_msg_open_drone_id_operator_id_send_struct(_chan, &pkt_operator_id);
557 : }
558 0 : }
559 :
560 : /*
561 : * This converts a horizontal accuracy float value to the corresponding enum
562 : *
563 : * @param Accuracy The horizontal accuracy in meters
564 : * @return Enum value representing the accuracy
565 : */
566 0 : MAV_ODID_HOR_ACC AP_OpenDroneID::create_enum_horizontal_accuracy(float accuracy) const
567 : {
568 : // Out of bounds return UNKNOWN flag
569 0 : if (accuracy < 0.0 || accuracy >= 18520.0) {
570 0 : return MAV_ODID_HOR_ACC_UNKNOWN;
571 : }
572 :
573 : static const struct {
574 : float accuracy; // Accuracy bound in meters
575 : MAV_ODID_HOR_ACC mavoutput; // mavlink enum output
576 : } horiz_accuracy_table[] = {
577 : { 1.0, MAV_ODID_HOR_ACC_1_METER},
578 : { 3.0, MAV_ODID_HOR_ACC_3_METER},
579 : {10.0, MAV_ODID_HOR_ACC_10_METER},
580 : {30.0, MAV_ODID_HOR_ACC_30_METER},
581 : {92.6, MAV_ODID_HOR_ACC_0_05NM},
582 : {185.2, MAV_ODID_HOR_ACC_0_1NM},
583 : {555.6, MAV_ODID_HOR_ACC_0_3NM},
584 : {926.0, MAV_ODID_HOR_ACC_0_5NM},
585 : {1852.0, MAV_ODID_HOR_ACC_1NM},
586 : {3704.0, MAV_ODID_HOR_ACC_2NM},
587 : {7408.0, MAV_ODID_HOR_ACC_4NM},
588 : {18520.0, MAV_ODID_HOR_ACC_10NM},
589 : };
590 :
591 0 : for (auto elem : horiz_accuracy_table) {
592 0 : if (accuracy < elem.accuracy) {
593 0 : return elem.mavoutput;
594 : }
595 : }
596 :
597 : // Should not reach this
598 0 : return MAV_ODID_HOR_ACC_UNKNOWN;
599 : }
600 :
601 : /**
602 : * This converts a vertical accuracy float value to the corresponding enum
603 : *
604 : * @param Accuracy The vertical accuracy in meters
605 : * @return Enum value representing the accuracy
606 : */
607 0 : MAV_ODID_VER_ACC AP_OpenDroneID::create_enum_vertical_accuracy(float accuracy) const
608 : {
609 : // Out of bounds return UNKNOWN flag
610 0 : if (accuracy < 0.0 || accuracy >= 150.0) {
611 0 : return MAV_ODID_VER_ACC_UNKNOWN;
612 : }
613 :
614 : static const struct {
615 : float accuracy; // Accuracy bound in meters
616 : MAV_ODID_VER_ACC mavoutput; // mavlink enum output
617 : } vertical_accuracy_table[] = {
618 : { 1.0, MAV_ODID_VER_ACC_1_METER},
619 : { 3.0, MAV_ODID_VER_ACC_3_METER},
620 : {10.0, MAV_ODID_VER_ACC_10_METER},
621 : {25.0, MAV_ODID_VER_ACC_25_METER},
622 : {45.0, MAV_ODID_VER_ACC_45_METER},
623 : {150.0, MAV_ODID_VER_ACC_150_METER},
624 : };
625 :
626 0 : for (auto elem : vertical_accuracy_table) {
627 0 : if (accuracy < elem.accuracy) {
628 0 : return elem.mavoutput;
629 : }
630 : }
631 :
632 : // Should not reach this
633 0 : return MAV_ODID_VER_ACC_UNKNOWN;
634 : }
635 :
636 : /**
637 : * This converts a speed accuracy float value to the corresponding enum
638 : *
639 : * @param Accuracy The speed accuracy in m/s
640 : * @return Enum value representing the accuracy
641 : */
642 0 : MAV_ODID_SPEED_ACC AP_OpenDroneID::create_enum_speed_accuracy(float accuracy) const
643 : {
644 : // Out of bounds return UNKNOWN flag
645 0 : if (accuracy < 0.0 || accuracy >= 10.0) {
646 0 : return MAV_ODID_SPEED_ACC_UNKNOWN;
647 : }
648 :
649 0 : if (accuracy < 0.3) {
650 0 : return MAV_ODID_SPEED_ACC_0_3_METERS_PER_SECOND;
651 0 : } else if (accuracy < 1.0) {
652 0 : return MAV_ODID_SPEED_ACC_1_METERS_PER_SECOND;
653 0 : } else if (accuracy < 3.0) {
654 0 : return MAV_ODID_SPEED_ACC_3_METERS_PER_SECOND;
655 0 : } else if (accuracy < 10.0) {
656 0 : return MAV_ODID_SPEED_ACC_10_METERS_PER_SECOND;
657 : }
658 :
659 : // Should not reach this
660 0 : return MAV_ODID_SPEED_ACC_UNKNOWN;
661 : }
662 :
663 : /**
664 : * This converts a timestamp accuracy float value to the corresponding enum
665 : *
666 : * @param Accuracy The timestamp accuracy in seconds
667 : * @return Enum value representing the accuracy
668 : */
669 0 : MAV_ODID_TIME_ACC AP_OpenDroneID::create_enum_timestamp_accuracy(float accuracy) const
670 : {
671 : // Out of bounds return UNKNOWN flag
672 0 : if (accuracy < 0.0 || accuracy >= 1.5) {
673 0 : return MAV_ODID_TIME_ACC_UNKNOWN;
674 : }
675 :
676 : static const MAV_ODID_TIME_ACC mavoutput [15] = {
677 : MAV_ODID_TIME_ACC_0_1_SECOND,
678 : MAV_ODID_TIME_ACC_0_2_SECOND,
679 : MAV_ODID_TIME_ACC_0_3_SECOND,
680 : MAV_ODID_TIME_ACC_0_4_SECOND,
681 : MAV_ODID_TIME_ACC_0_5_SECOND,
682 : MAV_ODID_TIME_ACC_0_6_SECOND,
683 : MAV_ODID_TIME_ACC_0_7_SECOND,
684 : MAV_ODID_TIME_ACC_0_8_SECOND,
685 : MAV_ODID_TIME_ACC_0_9_SECOND,
686 : MAV_ODID_TIME_ACC_1_0_SECOND,
687 : MAV_ODID_TIME_ACC_1_1_SECOND,
688 : MAV_ODID_TIME_ACC_1_2_SECOND,
689 : MAV_ODID_TIME_ACC_1_3_SECOND,
690 : MAV_ODID_TIME_ACC_1_4_SECOND,
691 : MAV_ODID_TIME_ACC_1_5_SECOND,
692 : };
693 :
694 0 : for (int8_t i = 1; i <= 15; i++) {
695 0 : if (accuracy <= 0.1 * i) {
696 0 : return mavoutput[i-1];
697 : }
698 : }
699 :
700 : // Should not reach this
701 0 : return MAV_ODID_TIME_ACC_UNKNOWN;
702 : }
703 :
704 : // make sure value is within limits of remote ID standard
705 0 : uint16_t AP_OpenDroneID::create_speed_horizontal(uint16_t speed) const
706 : {
707 0 : if (speed > ODID_MAX_SPEED_H) { // constraint function can't be used, because out of range value is invalid
708 0 : speed = ODID_INV_SPEED_H;
709 : }
710 :
711 0 : return speed;
712 : }
713 :
714 : // make sure value is within limits of remote ID standard
715 0 : int16_t AP_OpenDroneID::create_speed_vertical(int16_t speed) const
716 : {
717 0 : if (speed > ODID_MAX_SPEED_V) { // constraint function can't be used, because out of range value is invalid
718 0 : speed = ODID_INV_SPEED_V;
719 0 : } else if (speed < ODID_MIN_SPEED_V) {
720 0 : speed = ODID_INV_SPEED_V;
721 : }
722 :
723 0 : return speed;
724 : }
725 :
726 : // make sure value is within limits of remote ID standard
727 0 : float AP_OpenDroneID::create_altitude(float altitude) const
728 : {
729 0 : if (altitude > ODID_MAX_ALT) { // constraint function can't be used, because out of range value is invalid
730 0 : altitude = ODID_INV_ALT;
731 0 : } else if (altitude < ODID_MIN_ALT) {
732 0 : altitude = ODID_INV_ALT;
733 : }
734 :
735 0 : return altitude;
736 : }
737 :
738 : // make sure value is within limits of remote ID standard
739 0 : float AP_OpenDroneID::create_location_timestamp(float timestamp) const
740 : {
741 0 : if (timestamp > ODID_MAX_TIMESTAMP) { // constraint function can't be used, because out of range value is invalid
742 0 : timestamp = ODID_INV_TIMESTAMP;
743 0 : } else if (timestamp < 0) {
744 0 : timestamp = ODID_INV_TIMESTAMP;
745 : }
746 :
747 0 : return timestamp;
748 : }
749 :
750 : // handle a message from the GCS
751 0 : void AP_OpenDroneID::handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg)
752 : {
753 0 : if (!_initialised) {
754 0 : return;
755 : }
756 0 : WITH_SEMAPHORE(_sem);
757 :
758 0 : switch (msg.msgid) {
759 : // only accept ARM_STATUS from the transmitter
760 0 : case MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS: {
761 0 : if (chan == _chan) {
762 0 : mavlink_msg_open_drone_id_arm_status_decode(&msg, &arm_status);
763 0 : last_arm_status_ms = AP_HAL::millis();
764 : }
765 0 : break;
766 : }
767 : // accept other messages from the GCS
768 0 : case MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID:
769 0 : mavlink_msg_open_drone_id_operator_id_decode(&msg, &pkt_operator_id);
770 0 : break;
771 0 : case MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID:
772 0 : mavlink_msg_open_drone_id_self_id_decode(&msg, &pkt_self_id);
773 0 : break;
774 0 : case MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID:
775 0 : if (id_len == 0) {
776 0 : mavlink_msg_open_drone_id_basic_id_decode(&msg, &pkt_basic_id);
777 : }
778 0 : break;
779 0 : case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM:
780 0 : mavlink_msg_open_drone_id_system_decode(&msg, &pkt_system);
781 0 : last_system_ms = AP_HAL::millis();
782 0 : break;
783 0 : case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE: {
784 : mavlink_open_drone_id_system_update_t pkt_system_update;
785 0 : mavlink_msg_open_drone_id_system_update_decode(&msg, &pkt_system_update);
786 0 : pkt_system.operator_latitude = pkt_system_update.operator_latitude;
787 0 : pkt_system.operator_longitude = pkt_system_update.operator_longitude;
788 0 : pkt_system.operator_altitude_geo = pkt_system_update.operator_altitude_geo;
789 0 : pkt_system.timestamp = pkt_system_update.timestamp;
790 0 : last_system_update_ms = AP_HAL::millis();
791 0 : if (last_system_ms != 0) {
792 : // we can only mark system as updated if we have the other
793 : // information already
794 0 : last_system_ms = last_system_update_ms;
795 : }
796 0 : break;
797 : }
798 : }
799 0 : }
800 :
801 : // singleton instance
802 : AP_OpenDroneID *AP_OpenDroneID::_singleton;
803 :
804 : namespace AP
805 : {
806 :
807 24883001 : AP_OpenDroneID &opendroneid()
808 : {
809 24883001 : return *AP_OpenDroneID::get_singleton();
810 : }
811 :
812 : }
813 : #endif //AP_OPENDRONEID_ENABLED
|
