From 19fa512d4ee5e8ba548d85f66592a6d0ee7f95fb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 19 Jul 2024 14:14:09 +1200 Subject: [PATCH] remoteid: implement System as sent from GCS This will send the System message if it is already being sent by a ground station. Otherwise, it will assemble the message itself using the takeoff/home location. --- msg/CMakeLists.txt | 1 + msg/OpenDroneIdSystem.msg | 13 ++++ src/drivers/uavcan/remoteid.cpp | 87 ++++++++++++++++-------- src/drivers/uavcan/remoteid.hpp | 2 + src/modules/mavlink/mavlink_receiver.cpp | 29 ++++++++ src/modules/mavlink/mavlink_receiver.h | 2 + 6 files changed, 106 insertions(+), 28 deletions(-) create mode 100644 msg/OpenDroneIdSystem.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 7d7ab100d5ec..2f7ccd89229c 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -155,6 +155,7 @@ set(msg_files OpenDroneIdArmStatus.msg OpenDroneIdOperatorId.msg OpenDroneIdSelfId.msg + OpenDroneIdSystem.msg OrbitStatus.msg OrbTest.msg OrbTestLarge.msg diff --git a/msg/OpenDroneIdSystem.msg b/msg/OpenDroneIdSystem.msg new file mode 100644 index 000000000000..0604d8a42acd --- /dev/null +++ b/msg/OpenDroneIdSystem.msg @@ -0,0 +1,13 @@ +uint64 timestamp +uint8[20] id_or_mac +uint8 operator_location_type +uint8 classification_type +int32 operator_latitude +int32 operator_longitude +uint16 area_count +uint16 area_radius +float32 area_ceiling +float32 area_floor +uint8 category_eu +uint8 class_eu +float32 operator_altitude_geo diff --git a/src/drivers/uavcan/remoteid.cpp b/src/drivers/uavcan/remoteid.cpp index 6f66ba94cece..1b473e8d96fc 100644 --- a/src/drivers/uavcan/remoteid.cpp +++ b/src/drivers/uavcan/remoteid.cpp @@ -236,34 +236,65 @@ void UavcanRemoteIDController::send_location() void UavcanRemoteIDController::send_system() { - sensor_gps_s vehicle_gps_position; - home_position_s home_position; - - if (_vehicle_gps_position_sub.copy(&vehicle_gps_position) && _home_position_sub.copy(&home_position)) { - if (vehicle_gps_position.fix_type >= 3 - && home_position.valid_alt && home_position.valid_hpos) { - - dronecan::remoteid::System msg {}; - - // msg.id_or_mac // Only used for drone ID data received from other UAs. - msg.operator_location_type = MAV_ODID_OPERATOR_LOCATION_TYPE_TAKEOFF; - msg.classification_type = MAV_ODID_CLASSIFICATION_TYPE_UNDECLARED; - msg.operator_latitude = home_position.lat * 1e7; - msg.operator_longitude = home_position.lon * 1e7; - msg.area_count = 1; - msg.area_radius = 0; - msg.area_ceiling = -1000; - msg.area_floor = -1000; - msg.category_eu = MAV_ODID_CATEGORY_EU_UNDECLARED; - msg.class_eu = MAV_ODID_CLASS_EU_UNDECLARED; - float wgs84_amsl_offset = vehicle_gps_position.altitude_ellipsoid_m - vehicle_gps_position.altitude_msl_m; - msg.operator_altitude_geo = home_position.alt + wgs84_amsl_offset; - - // timestamp: 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. - static uint64_t utc_offset_s = 1'546'300'800; // UTC seconds since 00:00:00 01/01/2019 - msg.timestamp = vehicle_gps_position.time_utc_usec / 1e6 - utc_offset_s; - - _uavcan_pub_remoteid_system.broadcast(msg); + open_drone_id_system_s system; + + if (_open_drone_id_system.advertised() && _open_drone_id_system.copy(&system)) { + + // Use what ground station sends us. + + dronecan::remoteid::System msg {}; + msg.timestamp = system.timestamp; + + for (unsigned i = 0; i < sizeof(system.id_or_mac); ++i) { + msg.id_or_mac.push_back(system.id_or_mac[i]); + } + + msg.operator_location_type = system.operator_location_type; + msg.classification_type = system.classification_type; + msg.operator_latitude = system.operator_latitude; + msg.operator_longitude = system.operator_longitude; + msg.area_count = system.area_count; + msg.area_radius = system.area_radius; + msg.area_ceiling = system.area_ceiling; + msg.area_floor = system.area_floor; + msg.category_eu = system.category_eu; + msg.class_eu = system.class_eu; + msg.operator_altitude_geo = system.operator_altitude_geo; + + _uavcan_pub_remoteid_system.broadcast(msg); + + } else { + // And otherwise, send our home/takeoff location. + + sensor_gps_s vehicle_gps_position; + home_position_s home_position; + + if (_vehicle_gps_position_sub.copy(&vehicle_gps_position) && _home_position_sub.copy(&home_position)) { + if (vehicle_gps_position.fix_type >= 3 + && home_position.valid_alt && home_position.valid_hpos) { + + dronecan::remoteid::System msg {}; + + // msg.id_or_mac // Only used for drone ID data received from other UAs. + msg.operator_location_type = MAV_ODID_OPERATOR_LOCATION_TYPE_TAKEOFF; + msg.classification_type = MAV_ODID_CLASSIFICATION_TYPE_UNDECLARED; + msg.operator_latitude = home_position.lat * 1e7; + msg.operator_longitude = home_position.lon * 1e7; + msg.area_count = 1; + msg.area_radius = 0; + msg.area_ceiling = -1000; + msg.area_floor = -1000; + msg.category_eu = MAV_ODID_CATEGORY_EU_UNDECLARED; + msg.class_eu = MAV_ODID_CLASS_EU_UNDECLARED; + float wgs84_amsl_offset = vehicle_gps_position.altitude_ellipsoid_m - vehicle_gps_position.altitude_msl_m; + msg.operator_altitude_geo = home_position.alt + wgs84_amsl_offset; + + // timestamp: 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. + static uint64_t utc_offset_s = 1'546'300'800; // UTC seconds since 00:00:00 01/01/2019 + msg.timestamp = vehicle_gps_position.time_utc_usec / 1e6 - utc_offset_s; + + _uavcan_pub_remoteid_system.broadcast(msg); + } } } } diff --git a/src/drivers/uavcan/remoteid.hpp b/src/drivers/uavcan/remoteid.hpp index eb9bffd6c0e4..1d1fcaa83cef 100644 --- a/src/drivers/uavcan/remoteid.hpp +++ b/src/drivers/uavcan/remoteid.hpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -90,6 +91,7 @@ class UavcanRemoteIDController : public ModuleParams uORB::Subscription _home_position_sub{ORB_ID(home_position)}; uORB::Subscription _open_drone_id_operator_id{ORB_ID(open_drone_id_operator_id)}; uORB::Subscription _open_drone_id_self_id{ORB_ID(open_drone_id_self_id)}; + uORB::Subscription _open_drone_id_system{ORB_ID(open_drone_id_system)}; uORB::Publication _open_drone_id_arm_status_pub{ORB_ID(open_drone_id_arm_status)}; uavcan::Publisher _uavcan_pub_remoteid_basicid; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 24b90e9f3b22..f5f5d764d2fd 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -288,6 +288,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_open_drone_id_self_id(msg); break; + case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM: + handle_message_open_drone_id_system(msg); + break; + #if !defined(CONSTRAINED_FLASH) case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: @@ -3089,6 +3093,31 @@ void MavlinkReceiver::handle_message_open_drone_id_self_id(mavlink_message_t *ms _open_drone_id_self_id_pub.publish(odid_self_id); } +void MavlinkReceiver::handle_message_open_drone_id_system( + mavlink_message_t *msg) +{ + mavlink_open_drone_id_system_t odid_module; + mavlink_msg_open_drone_id_system_decode(msg, &odid_module); + + open_drone_id_system_s odid_system{}; + memset(&odid_system, 0, sizeof(odid_system)); + + odid_system.timestamp = hrt_absolute_time(); + memcpy(odid_system.id_or_mac, odid_module.id_or_mac, sizeof(odid_system.id_or_mac)); + odid_system.operator_location_type = odid_module.operator_location_type; + odid_system.classification_type = odid_module.classification_type; + odid_system.operator_latitude = odid_module.operator_latitude; + odid_system.operator_longitude = odid_module.operator_longitude; + odid_system.area_count = odid_module.area_count; + odid_system.area_radius = odid_module.area_radius; + odid_system.area_ceiling = odid_module.area_ceiling; + odid_system.area_floor = odid_module.area_floor; + odid_system.category_eu = odid_module.category_eu; + odid_system.class_eu = odid_module.class_eu; + odid_system.operator_altitude_geo = odid_module.operator_altitude_geo; + + _open_drone_id_system_pub.publish(odid_system); +} void MavlinkReceiver::run() { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index af67cbe5f3c6..76e7fac8ec83 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -89,6 +89,7 @@ #include #include #include +#include #include #include #include @@ -318,6 +319,7 @@ class MavlinkReceiver : public ModuleParams uORB::Publication _velocity_limits_pub{ORB_ID(velocity_limits)}; uORB::Publication _open_drone_id_operator_id_pub{ORB_ID(open_drone_id_operator_id)}; uORB::Publication _open_drone_id_self_id_pub{ORB_ID(open_drone_id_self_id)}; + uORB::Publication _open_drone_id_system_pub{ORB_ID(open_drone_id_system)}; uORB::Publication _generator_status_pub{ORB_ID(generator_status)}; uORB::Publication _attitude_pub{ORB_ID(vehicle_attitude)}; uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};