diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index f161085e64a4..015436541498 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -138,6 +138,9 @@ set(msg_files OffboardControlMode.msg OnboardComputerStatus.msg OpenDroneIdArmStatus.msg + OpenDroneIdOperatorId.msg + OpenDroneIdSelfId.msg + OpenDroneIdSystem.msg OrbitStatus.msg OrbTest.msg OrbTestLarge.msg diff --git a/msg/OpenDroneIdOperatorId.msg b/msg/OpenDroneIdOperatorId.msg new file mode 100644 index 000000000000..d75a419717f5 --- /dev/null +++ b/msg/OpenDroneIdOperatorId.msg @@ -0,0 +1,6 @@ +uint64 timestamp +uint8 target_system +uint8 target_component +uint8[20] id_or_mac +uint8 operator_id_type +char[20] operator_id diff --git a/msg/OpenDroneIdSelfId.msg b/msg/OpenDroneIdSelfId.msg new file mode 100644 index 000000000000..a176f94a87dc --- /dev/null +++ b/msg/OpenDroneIdSelfId.msg @@ -0,0 +1,6 @@ +uint64 timestamp +uint8 target_system +uint8 target_component +uint8[20] id_or_mac +uint8 description_type +char[23] description diff --git a/msg/OpenDroneIdSystem.msg b/msg/OpenDroneIdSystem.msg new file mode 100644 index 000000000000..bcc5a8c42036 --- /dev/null +++ b/msg/OpenDroneIdSystem.msg @@ -0,0 +1,15 @@ +uint64 timestamp +uint8 target_system +uint8 target_component +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/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 500339290161..d540010ecd36 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -288,6 +288,18 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_open_drone_id_arm_status(msg); break; + case MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID: + handle_message_open_drone_id_operator_id(msg); + break; + + case MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID: + 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: @@ -3088,6 +3100,72 @@ void MavlinkReceiver::handle_message_open_drone_id_arm_status( _open_drone_id_arm_status_pub.publish(odid_arm); } + +void MavlinkReceiver::handle_message_open_drone_id_operator_id( + mavlink_message_t *msg) +{ + + mavlink_open_drone_id_operator_id_t odid_module; + mavlink_msg_open_drone_id_operator_id_decode(msg, &odid_module); + + open_drone_id_operator_id_s odid_operator_id{}; + memset(&odid_operator_id, 0, sizeof(odid_operator_id)); + + odid_operator_id.timestamp = hrt_absolute_time(); + odid_operator_id.target_system = odid_module.target_system; + odid_operator_id.target_component = odid_module.target_component; + memcpy(odid_operator_id.id_or_mac, odid_module.id_or_mac, sizeof(odid_operator_id.id_or_mac)); + odid_operator_id.operator_id_type = odid_module.operator_id_type; + memcpy(odid_operator_id.operator_id, odid_module.operator_id, sizeof(odid_operator_id.operator_id)); + + _open_drone_id_operator_id_pub.publish(odid_operator_id); +} + +void MavlinkReceiver::handle_message_open_drone_id_self_id( + mavlink_message_t *msg) +{ + mavlink_open_drone_id_self_id_t odid_module; + mavlink_msg_open_drone_id_self_id_decode(msg, &odid_module); + + open_drone_id_self_id_s odid_self_id{}; + memset(&odid_self_id, 0, sizeof(odid_self_id)); + + odid_self_id.timestamp = hrt_absolute_time(); + odid_self_id.target_system = odid_module.target_system; + odid_self_id.target_component = odid_module.target_component; + memcpy(odid_self_id.id_or_mac, odid_module.id_or_mac, sizeof(odid_self_id.id_or_mac)); + odid_self_id.description_type = odid_module.description_type; + memcpy(odid_self_id.description, odid_module.description, sizeof(odid_self_id.description)); + + _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(); + odid_system.target_system = odid_module.target_system; + odid_system.target_component = odid_module.target_component; + 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; +} + void MavlinkReceiver::run() { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index d6ee5a976569..6c380e4ad5d2 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -88,6 +88,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -181,6 +184,9 @@ class MavlinkReceiver : public ModuleParams void handle_message_odometry(mavlink_message_t *msg); void handle_message_onboard_computer_status(mavlink_message_t *msg); void handle_message_open_drone_id_arm_status(mavlink_message_t *msg); + void handle_message_open_drone_id_operator_id(mavlink_message_t *msg); + void handle_message_open_drone_id_self_id(mavlink_message_t *msg); + void handle_message_open_drone_id_system(mavlink_message_t *msg); void handle_message_optical_flow_rad(mavlink_message_t *msg); void handle_message_ping(mavlink_message_t *msg); void handle_message_play_tune(mavlink_message_t *msg); @@ -308,6 +314,9 @@ class MavlinkReceiver : public ModuleParams uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; uORB::Publication _onboard_computer_status_pub{ORB_ID(onboard_computer_status)}; uORB::Publication _open_drone_id_arm_status_pub{ORB_ID(open_drone_id_arm_status)}; + 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)}; diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_OPERATOR_ID.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_OPERATOR_ID.hpp index fd8ec6e071b7..e205e1f32777 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_OPERATOR_ID.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_OPERATOR_ID.hpp @@ -35,6 +35,7 @@ #define OPEN_DRONE_ID_OPERATOR_ID_HPP #include +#include class MavlinkStreamOpenDroneIdOperatorId : public MavlinkStream { @@ -68,23 +69,31 @@ class MavlinkStreamOpenDroneIdOperatorId : public MavlinkStream : MavlinkStream(mavlink) {} uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _open_drone_id_operator_id_sub{ORB_ID(open_drone_id_operator_id)}; bool send() override { - mavlink_open_drone_id_operator_id_t msg{}; - msg.target_component = 0; // 0 for broadcast - msg.target_system = 0; // 0 for broadcast - // msg.id_or_mac // Only used for drone ID data received from other UAs. - msg.operator_id_type = 0; - - for (uint8_t i = 0; i < 20; ++i) { - msg.operator_id[i] = '\0'; - } + open_drone_id_operator_id_s operator_id{}; + + if(_open_drone_id_operator_id_sub.update(&operator_id)) { + + mavlink_open_drone_id_operator_id_t msg{}; + msg.target_system = 0; // 0 for broadcast + msg.target_component = 0; // 0 for broadcast + // msg.id_or_mac // Only used for drone ID data received from other UAs. + msg.operator_id_type = operator_id.operator_id_type; - mavlink_msg_open_drone_id_operator_id_send_struct(_mavlink->get_channel(), - &msg); + for (uint8_t i = 0; i < 20; ++i) { + msg.operator_id[i] = operator_id.operator_id[i]; + } + + mavlink_msg_open_drone_id_operator_id_send_struct(_mavlink->get_channel(), + &msg); + + return true; + } - return true; + return false; } }; diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_SELF_ID.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_SELF_ID.hpp index 6c0af7f2269c..fe19136394d0 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_SELF_ID.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_SELF_ID.hpp @@ -35,6 +35,7 @@ #define OPEN_DRONE_ID_SELF_ID_HPP #include +#include class MavlinkStreamOpenDroneIdSelfId : public MavlinkStream { @@ -67,23 +68,31 @@ class MavlinkStreamOpenDroneIdSelfId : public MavlinkStream : MavlinkStream(mavlink) {} uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _open_drone_id_self_id_sub{ORB_ID(open_drone_id_self_id)}; bool send() override { - mavlink_open_drone_id_self_id_t msg{}; - msg.target_component = 0; // 0 for broadcast - msg.target_system = 0; // 0 for broadcast - // msg.id_or_mac // Only used for drone ID data received from other UAs. - msg.description_type = 0; - - for (uint8_t i = 0; i < 23; ++i) { - msg.description[i] = '\0'; - } + open_drone_id_self_id_s self_id; + + if(_open_drone_id_self_id_sub.update(&self_id)) { + + mavlink_open_drone_id_self_id_t msg{}; + msg.target_system = 0; // 0 for broadcast + msg.target_component = 0; // 0 for broadcast + // msg.id_or_mac // Only used for drone ID data received from other UAs. + msg.description_type = self_id.description_type; - mavlink_msg_open_drone_id_self_id_send_struct(_mavlink->get_channel(), - &msg); + for (uint8_t i = 0; i < 23; ++i) { + msg.description[i] = self_id.description[i]; + } + + mavlink_msg_open_drone_id_self_id_send_struct(_mavlink->get_channel(), + &msg); + + return true; + } - return true; + return false; } }; diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp index 8f73ec39aa95..5600f1dd38ae 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp @@ -36,6 +36,7 @@ #include #include +#include class MavlinkStreamOpenDroneIdSystem : public MavlinkStream { @@ -74,20 +75,21 @@ class MavlinkStreamOpenDroneIdSystem : public MavlinkStream uORB::Subscription _home_position_sub{ORB_ID(home_position)}; uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::Subscription _open_drone_id_system_sub{ORB_ID(open_drone_id_system)}; bool send() override { sensor_gps_s vehicle_gps_position; home_position_s home_position; - + open_drone_id_system_s odid_system; if (_vehicle_gps_position_sub.update(&vehicle_gps_position) && _home_position_sub.copy(&home_position)) { if (vehicle_gps_position.fix_type >= 3 && home_position.valid_alt && home_position.valid_hpos) { mavlink_open_drone_id_system_t msg{}; - msg.target_component = 0; // 0 for broadcast msg.target_system = 0; // 0 for broadcast + msg.target_component = 0; // 0 for broadcast // 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; @@ -114,6 +116,36 @@ class MavlinkStreamOpenDroneIdSystem : public MavlinkStream } } + + if (_vehicle_gps_position_sub.update(&vehicle_gps_position) && + _home_position_sub.copy(&home_position) && _open_drone_id_system_sub.update(&odid_system)) { + if (vehicle_gps_position.fix_type >= 3 && home_position.valid_alt && + home_position.valid_hpos) { + + mavlink_open_drone_id_system_t msg{}; + + msg.target_system = 0; // 0 for broadcast + msg.target_component = 0; // 0 for broadcast + // msg.id_or_mac // Only used for drone ID data received from other UAs. + msg.operator_location_type = odid_system.operator_location_type; + msg.classification_type = odid_system.classification_type; + msg.operator_latitude = odid_system.operator_latitude; + msg.operator_longitude = odid_system.operator_longitude; + msg.area_count = odid_system.area_count; + msg.area_radius = odid_system.area_radius; + msg.area_ceiling = odid_system.area_ceiling; + msg.area_floor = odid_system.area_floor; + msg.category_eu = odid_system.category_eu; + msg.class_eu = odid_system.class_eu; + msg.operator_altitude_geo = odid_system.operator_altitude_geo; + + msg.timestamp = odid_system.timestamp; + + mavlink_msg_open_drone_id_system_send_struct(_mavlink->get_channel(), + &msg); + return true; + } + } return false; } };