diff --git a/boards/ark/fmu-v6x/default.px4board b/boards/ark/fmu-v6x/default.px4board index 92bf7cb23895..74d63e4cc33c 100644 --- a/boards/ark/fmu-v6x/default.px4board +++ b/boards/ark/fmu-v6x/default.px4board @@ -85,8 +85,6 @@ CONFIG_SYSTEMCMDS_NSHTERM=y CONFIG_SYSTEMCMDS_PARAM=y CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_REBOOT=y -CONFIG_SYSTEMCMDS_SD_BENCH=y -CONFIG_SYSTEMCMDS_SD_STRESS=y CONFIG_SYSTEMCMDS_SYSTEM_TIME=y CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y diff --git a/boards/cuav/x7pro/default.px4board b/boards/cuav/x7pro/default.px4board index f72610fc65cf..4ba4d4a17754 100644 --- a/boards/cuav/x7pro/default.px4board +++ b/boards/cuav/x7pro/default.px4board @@ -79,7 +79,6 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_BSONDUMP=y CONFIG_SYSTEMCMDS_DMESG=y -CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_GPIO=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y CONFIG_SYSTEMCMDS_I2CDETECT=y @@ -91,7 +90,6 @@ CONFIG_SYSTEMCMDS_PARAM=y CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_REBOOT=y CONFIG_SYSTEMCMDS_SD_BENCH=y -CONFIG_SYSTEMCMDS_SD_STRESS=y CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y CONFIG_SYSTEMCMDS_TUNE_CONTROL=y @@ -99,4 +97,3 @@ CONFIG_SYSTEMCMDS_UORB=y CONFIG_SYSTEMCMDS_USB_CONNECTED=y CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y -CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/px4/fmu-v4pro/test.px4board b/boards/px4/fmu-v4pro/test.px4board index 057de060c021..51d1135a96cf 100644 --- a/boards/px4/fmu-v4pro/test.px4board +++ b/boards/px4/fmu-v4pro/test.px4board @@ -3,6 +3,8 @@ CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n CONFIG_DRIVERS_IRLOCK=n CONFIG_DRIVERS_PCA9685_PWM_OUT=n CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_GYRO_FFT=n CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n CONFIG_BOARD_TESTING=y diff --git a/boards/px4/fmu-v5/cryptotest.px4board b/boards/px4/fmu-v5/cryptotest.px4board index 831abbb824ab..cc31ed3c2e50 100644 --- a/boards/px4/fmu-v5/cryptotest.px4board +++ b/boards/px4/fmu-v5/cryptotest.px4board @@ -1,4 +1,6 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_BOARD_CRYPTO=y CONFIG_DRIVERS_STUB_KEYSTORE=y CONFIG_DRIVERS_SW_CRYPTO=y diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index a48b0956876b..8ba41726c086 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -29,7 +29,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y -CONFIG_COMMON_INS=y CONFIG_DRIVERS_IRLOCK=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index 7c4bf0b6f6cb..eedfd4c90ecb 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -30,7 +30,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y -CONFIG_COMMON_INS=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_DRIVERS_OSD_MSP_OSD=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index dce7e851dca9..91c38488f5aa 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -152,6 +152,10 @@ set(msg_files ObstacleDistance.msg OffboardControlMode.msg OnboardComputerStatus.msg + OpenDroneIdArmStatus.msg + OpenDroneIdOperatorId.msg + OpenDroneIdSelfId.msg + OpenDroneIdSystem.msg OrbitStatus.msg OrbTest.msg OrbTestLarge.msg diff --git a/msg/OpenDroneIdArmStatus.msg b/msg/OpenDroneIdArmStatus.msg new file mode 100644 index 000000000000..1fa58c6a1f26 --- /dev/null +++ b/msg/OpenDroneIdArmStatus.msg @@ -0,0 +1,3 @@ +uint64 timestamp +uint8 status +char[50] error diff --git a/msg/OpenDroneIdOperatorId.msg b/msg/OpenDroneIdOperatorId.msg new file mode 100644 index 000000000000..0f78a47e5a8d --- /dev/null +++ b/msg/OpenDroneIdOperatorId.msg @@ -0,0 +1,4 @@ +uint64 timestamp +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..1ff699ebf142 --- /dev/null +++ b/msg/OpenDroneIdSelfId.msg @@ -0,0 +1,4 @@ +uint64 timestamp +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..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/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index c70d029ea7e3..f8bc1eea8a6a 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -147,6 +147,8 @@ px4_add_module( arming_status.hpp beep.cpp beep.hpp + remoteid.cpp + remoteid.hpp rgbled.cpp rgbled.hpp safety_state.cpp diff --git a/src/drivers/uavcan/Kconfig b/src/drivers/uavcan/Kconfig index 0076d36568f5..37306dabdfde 100644 --- a/src/drivers/uavcan/Kconfig +++ b/src/drivers/uavcan/Kconfig @@ -26,6 +26,10 @@ if DRIVERS_UAVCAN bool "Include safety state controller" default y + config UAVCAN_REMOTEID_CONTROLLER + bool "Include remote ID controller" + default y + config UAVCAN_RGB_CONTROLLER bool "Include rgb controller" default y diff --git a/src/drivers/uavcan/libuavcan b/src/drivers/uavcan/libuavcan index 9a0fd624c448..dce2d4e7d8f4 160000 --- a/src/drivers/uavcan/libuavcan +++ b/src/drivers/uavcan/libuavcan @@ -1 +1 @@ -Subproject commit 9a0fd624c448cad5633720676233f74846387a9f +Subproject commit dce2d4e7d8f41348e571481cd2e4788ac8af900d diff --git a/src/drivers/uavcan/remoteid.cpp b/src/drivers/uavcan/remoteid.cpp new file mode 100644 index 000000000000..a2f373b767b0 --- /dev/null +++ b/src/drivers/uavcan/remoteid.cpp @@ -0,0 +1,356 @@ +/**************************************************************************** + * + * Copyright (C) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "remoteid.hpp" +#include +#include + +using namespace time_literals; + + +UavcanRemoteIDController::UavcanRemoteIDController(uavcan::INode &node) : + ModuleParams(nullptr), + _timer(node), + _node(node), + _uavcan_pub_remoteid_basicid(node), + _uavcan_pub_remoteid_location(node), + _uavcan_pub_remoteid_self_id(node), + _uavcan_pub_remoteid_system(node), + _uavcan_pub_remoteid_operator_id(node), + _uavcan_sub_arm_status(node) +{ +} + +int UavcanRemoteIDController::init() +{ + // Setup timer and call back function for periodic updates + _timer.setCallback(TimerCbBinder(this, &UavcanRemoteIDController::periodic_update)); + _timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ)); + + int res = _uavcan_sub_arm_status.start(ArmStatusBinder(this, &UavcanRemoteIDController::arm_status_sub_cb)); + + if (res < 0) { + PX4_WARN("ArmStatus sub failed %i", res); + return res; + } + + return 0; +} + +void UavcanRemoteIDController::periodic_update(const uavcan::TimerEvent &) +{ + _vehicle_status.update(); + + send_basic_id(); + send_location(); + send_self_id(); + send_system(); + send_operator_id(); +} + +void UavcanRemoteIDController::send_basic_id() +{ + dronecan::remoteid::BasicID basic_id {}; + // basic_id.id_or_mac // supposedly only used for drone ID data from other UAs + basic_id.id_type = dronecan::remoteid::BasicID::ODID_ID_TYPE_SERIAL_NUMBER; + basic_id.ua_type = static_cast(open_drone_id_translations::odidTypeForMavType( + _vehicle_status.get().system_type)); + + // uas_id: UAS (Unmanned Aircraft System) ID following the format specified by id_type + // TODO: MAV_ODID_ID_TYPE_SERIAL_NUMBER needs to be ANSI/CTA-2063 format + + char uas_id[20] = {}; + board_get_px4_guid_formated((char *)(uas_id), sizeof(uas_id)); + basic_id.uas_id = uas_id; + + _uavcan_pub_remoteid_basicid.broadcast(basic_id); +} + +void UavcanRemoteIDController::send_location() +{ + dronecan::remoteid::Location msg {}; + + // initialize all fields to unknown + msg.status = MAV_ODID_STATUS_UNDECLARED; + msg.direction = 36100; // If unknown: 36100 centi-degrees + msg.speed_horizontal = 25500; // If unknown: 25500 cm/s + msg.speed_vertical = 6300; // If unknown: 6300 cm/s + msg.latitude = 0; // If unknown: 0 + msg.longitude = 0; // If unknown: 0 + msg.altitude_geodetic = -1000; // If unknown: -1000 m + msg.altitude_geodetic = -1000; // If unknown: -1000 m + msg.height = -1000; // If unknown: -1000 m + msg.horizontal_accuracy = MAV_ODID_HOR_ACC_UNKNOWN; + msg.vertical_accuracy = MAV_ODID_VER_ACC_UNKNOWN; + msg.barometer_accuracy = MAV_ODID_VER_ACC_UNKNOWN; + msg.speed_accuracy = MAV_ODID_SPEED_ACC_UNKNOWN; + msg.timestamp = 0xFFFF; // If unknown: 0xFFFF + msg.timestamp_accuracy = MAV_ODID_TIME_ACC_UNKNOWN; + + bool updated = false; + + if (_vehicle_land_detected_sub.advertised()) { + vehicle_land_detected_s vehicle_land_detected{}; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected) + && (hrt_elapsed_time(&vehicle_land_detected.timestamp) < 10_s)) { + if (vehicle_land_detected.landed) { + msg.status = MAV_ODID_STATUS_GROUND; + + } else { + msg.status = MAV_ODID_STATUS_AIRBORNE; + } + + updated = true; + } + } + + if (hrt_elapsed_time(&_vehicle_status.get().timestamp) < 10_s) { + if (_vehicle_status.get().failsafe && (_vehicle_status.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { + msg.status = MAV_ODID_STATUS_EMERGENCY; + updated = true; + } + } + + if (_vehicle_gps_position_sub.advertised()) { + sensor_gps_s vehicle_gps_position{}; + + if (_vehicle_gps_position_sub.copy(&vehicle_gps_position) + && (hrt_elapsed_time(&vehicle_gps_position.timestamp) < 10_s)) { + + if (vehicle_gps_position.vel_ned_valid) { + const matrix::Vector3f vel_ned{vehicle_gps_position.vel_n_m_s, vehicle_gps_position.vel_e_m_s, vehicle_gps_position.vel_d_m_s}; + + // direction: calculate GPS course over ground angle + const float course = atan2f(vel_ned(1), vel_ned(0)); + const int course_deg = roundf(math::degrees(matrix::wrap_2pi(course))); + msg.direction = math::constrain(100 * course_deg, 0, 35999); // 0 - 35999 centi-degrees + + // speed_horizontal: If speed is larger than 25425 cm/s, use 25425 cm/s. + const int speed_horizontal_cm_s = matrix::Vector2f(vel_ned).length() * 100.f; + msg.speed_horizontal = math::constrain(speed_horizontal_cm_s, 0, 25425); + + // speed_vertical: Up is positive, If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s. + const int speed_vertical_cm_s = roundf(-vel_ned(2) * 100.f); + msg.speed_vertical = math::constrain(speed_vertical_cm_s, -6200, 6200); + + msg.speed_accuracy = open_drone_id_translations::odidSpeedAccForVariance(vehicle_gps_position.s_variance_m_s); + + updated = true; + } + + if (vehicle_gps_position.fix_type >= 2) { + msg.latitude = static_cast(round(vehicle_gps_position.latitude_deg * 1e7)); + msg.longitude = static_cast(round(vehicle_gps_position.longitude_deg * 1e7)); + + // altitude_geodetic + if (vehicle_gps_position.fix_type >= 3) { + msg.altitude_geodetic = static_cast(round(vehicle_gps_position.altitude_msl_m)); // [m] + } + + msg.horizontal_accuracy = open_drone_id_translations::odidHorAccForEph(vehicle_gps_position.eph); + + msg.vertical_accuracy = open_drone_id_translations::odidVerAccForEpv(vehicle_gps_position.epv); + + updated = true; + } + + if (vehicle_gps_position.time_utc_usec != 0) { + // timestamp: UTC then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000 + uint64_t utc_time_msec = vehicle_gps_position.time_utc_usec / 1000; + msg.timestamp = ((float)(utc_time_msec % (60 * 60 * 1000))) / 1000; + + msg.timestamp_accuracy = open_drone_id_translations::odidTimeForElapsed(hrt_elapsed_time( + &vehicle_gps_position.timestamp)); + + updated = true; + } + } + } + + // altitude_barometric: The altitude calculated from the barometric pressue + if (_vehicle_air_data_sub.advertised()) { + vehicle_air_data_s vehicle_air_data{}; + + if (_vehicle_air_data_sub.copy(&vehicle_air_data) && (hrt_elapsed_time(&vehicle_air_data.timestamp) < 10_s)) { + msg.altitude_barometric = vehicle_air_data.baro_alt_meter; + msg.barometer_accuracy = MAV_ODID_VER_ACC_UNKNOWN; // We just don't without calibration. + updated = true; + } + } + + // height: The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference + if (_home_position_sub.advertised() && _vehicle_local_position_sub.updated()) { + home_position_s home_position{}; + vehicle_local_position_s vehicle_local_position{}; + + if (_home_position_sub.copy(&home_position) + && _vehicle_local_position_sub.copy(&vehicle_local_position) + && (hrt_elapsed_time(&vehicle_local_position.timestamp) < 1_s) + ) { + + if (home_position.valid_alt && vehicle_local_position.z_valid && vehicle_local_position.z_global) { + float altitude = (-vehicle_local_position.z + vehicle_local_position.ref_alt); + + msg.height = altitude - home_position.alt; + msg.height_reference = MAV_ODID_HEIGHT_REF_OVER_TAKEOFF; + updated = true; + } + } + } + + if (updated) { + _uavcan_pub_remoteid_location.broadcast(msg); + } +} + +void UavcanRemoteIDController::send_system() +{ + 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); + } + } + } +} + +void UavcanRemoteIDController::send_self_id() +{ + open_drone_id_self_id_s self_id; + + if (_open_drone_id_self_id.copy(&self_id)) { + + dronecan::remoteid::SelfID msg {}; + + for (unsigned i = 0; i < sizeof(self_id.id_or_mac); ++i) { + msg.id_or_mac.push_back(self_id.id_or_mac[i]); + } + + msg.description_type = self_id.description_type; + + for (unsigned i = 0; i < sizeof(self_id.description); ++i) { + msg.description.push_back(self_id.description[i]); + } + + _uavcan_pub_remoteid_self_id.broadcast(msg); + } +} + +void UavcanRemoteIDController::send_operator_id() +{ + open_drone_id_operator_id_s operator_id; + + if (_open_drone_id_operator_id.copy(&operator_id)) { + + dronecan::remoteid::OperatorID msg {}; + + for (unsigned i = 0; i < sizeof(operator_id.id_or_mac); ++i) { + msg.id_or_mac.push_back(operator_id.id_or_mac[i]); + } + + msg.operator_id_type = operator_id.operator_id_type; + + for (unsigned i = 0; i < sizeof(operator_id.operator_id); ++i) { + msg.operator_id.push_back(operator_id.operator_id[i]); + } + + _uavcan_pub_remoteid_operator_id.broadcast(msg); + } +} + +void +UavcanRemoteIDController::arm_status_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + open_drone_id_arm_status_s arm_status{}; + + arm_status.timestamp = hrt_absolute_time(); + arm_status.status = msg.status; + memcpy(arm_status.error, msg.error.c_str(), sizeof(arm_status.error)); + + _open_drone_id_arm_status_pub.publish(arm_status); +} diff --git a/src/drivers/uavcan/remoteid.hpp b/src/drivers/uavcan/remoteid.hpp new file mode 100644 index 000000000000..1d1fcaa83cef --- /dev/null +++ b/src/drivers/uavcan/remoteid.hpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (C) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +class UavcanRemoteIDController : public ModuleParams +{ +public: + UavcanRemoteIDController(uavcan::INode &node); + ~UavcanRemoteIDController() = default; + + int init(); + +private: + typedef uavcan::MethodBinder + TimerCbBinder; + + static constexpr unsigned MAX_RATE_HZ = 1; + uavcan::TimerEventForwarder _timer; + + void periodic_update(const uavcan::TimerEvent &); + + void send_basic_id(); + void send_location(); + void send_self_id(); + void send_system(); + void send_operator_id(); + + void arm_status_sub_cb(const uavcan::ReceivedDataStructure &msg); + + uavcan::INode &_node; + + uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::SubscriptionData _vehicle_status{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + 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; + uavcan::Publisher _uavcan_pub_remoteid_location; + uavcan::Publisher _uavcan_pub_remoteid_self_id; + uavcan::Publisher _uavcan_pub_remoteid_system; + uavcan::Publisher _uavcan_pub_remoteid_operator_id; + + using ArmStatusBinder = uavcan::MethodBinder < UavcanRemoteIDController *, + void (UavcanRemoteIDController::*)(const uavcan::ReceivedDataStructure &) >; + + uavcan::Subscriber _uavcan_sub_arm_status; +}; diff --git a/src/drivers/uavcan/rgbled.hpp b/src/drivers/uavcan/rgbled.hpp index b1190214b111..be0bc876d38b 100644 --- a/src/drivers/uavcan/rgbled.hpp +++ b/src/drivers/uavcan/rgbled.hpp @@ -51,7 +51,7 @@ class UavcanRGBController : public ModuleParams int init(); private: - // Max update rate to avoid exessive bus traffic + // Max update rate to avoid excessive bus traffic static constexpr unsigned MAX_RATE_HZ = 20; void periodic_update(const uavcan::TimerEvent &); diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index fbdc534a9c30..87f3110cac7d 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -96,6 +96,9 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys #if defined(CONFIG_UAVCAN_SAFETY_STATE_CONTROLLER) _safety_state_controller(_node), #endif +#if defined(CONFIG_UAVCAN_REMOTEID_CONTROLLER) + _remoteid_controller(_node), +#endif #if defined(CONFIG_UAVCAN_RGB_CONTROLLER) _rgbled_controller(_node), #endif @@ -558,6 +561,16 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) return ret; } +#if defined(CONFIG_UAVCAN_REMOTEID_CONTROLLER) + ret = _remoteid_controller.init(); + + if (ret < 0) { + return ret; + } + +#endif + + #if defined(CONFIG_UAVCAN_RGB_CONTROLLER) ret = _rgbled_controller.init(); diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 5afa04ef672e..d249e5d838cc 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -68,6 +68,10 @@ #include "logmessage.hpp" +#if defined(CONFIG_UAVCAN_REMOTEID_CONTROLLER) +#include "remoteid.hpp" +#endif + #if defined(CONFIG_UAVCAN_RGB_CONTROLLER) #include "rgbled.hpp" #endif @@ -269,6 +273,9 @@ class UavcanNode : public px4::ScheduledWorkItem, public ModuleParams #if defined(CONFIG_UAVCAN_SAFETY_STATE_CONTROLLER) UavcanSafetyState _safety_state_controller; #endif +#if defined(CONFIG_UAVCAN_REMOTEID_CONTROLLER) + UavcanRemoteIDController _remoteid_controller; +#endif #if defined(CONFIG_UAVCAN_RGB_CONTROLLER) UavcanRGBController _rgbled_controller; #endif diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index f42dd6fa9b7b..45aa9b1cc82f 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -120,6 +120,7 @@ px4_add_module( mavlink_timesync.cpp mavlink_ulog.cpp MavlinkStatustextHandler.cpp + open_drone_id_translations.cpp tune_publisher.cpp MODULE_CONFIG module.yaml diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 54e1a8549eeb..e0a4a9c2baa6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1429,6 +1429,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("OBSTACLE_DISTANCE", 1.0f); configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f); configure_stream_local("OPEN_DRONE_ID_SYSTEM", 1.f); + configure_stream_local("OPEN_DRONE_ID_ARM_STATUS", 1.f); configure_stream_local("ORBIT_EXECUTION_STATUS", 2.0f); configure_stream_local("PING", 0.1f); configure_stream_local("POSITION_TARGET_GLOBAL_INT", 1.0f); @@ -1498,6 +1499,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f); configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f); configure_stream_local("OPEN_DRONE_ID_SYSTEM", 1.f); + configure_stream_local("OPEN_DRONE_ID_ARM_STATUS", 1.f); configure_stream_local("OPTICAL_FLOW_RAD", 10.0f); configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f); configure_stream_local("PING", 1.0f); @@ -1666,6 +1668,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f); configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f); configure_stream_local("OPEN_DRONE_ID_SYSTEM", 1.f); + configure_stream_local("OPEN_DRONE_ID_ARM_STATUS", 1.f); configure_stream_local("OPTICAL_FLOW_RAD", 10.0f); configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f); configure_stream_local("PING", 1.0f); @@ -1758,6 +1761,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.5f); configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f); configure_stream_local("OPEN_DRONE_ID_SYSTEM", 1.f); + configure_stream_local("OPEN_DRONE_ID_ARM_STATUS", 1.f); configure_stream_local("OPTICAL_FLOW_RAD", 1.0f); configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f); configure_stream_local("PING", 0.1f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 638de9cce5f4..60f021a5b126 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -97,6 +97,7 @@ #include "streams/OPEN_DRONE_ID_BASIC_ID.hpp" #include "streams/OPEN_DRONE_ID_LOCATION.hpp" #include "streams/OPEN_DRONE_ID_SYSTEM.hpp" +#include "streams/OPEN_DRONE_ID_ARM_STATUS.hpp" #include "streams/OPTICAL_FLOW_RAD.hpp" #include "streams/ORBIT_EXECUTION_STATUS.hpp" #include "streams/PING.hpp" @@ -457,6 +458,9 @@ static const StreamListItem streams_list[] = { #if defined(OPEN_DRONE_ID_SYSTEM_HPP) create_stream_list_item(), #endif // OPEN_DRONE_ID_SYSTEM_HPP +#if defined(OPEN_DRONE_ID_ARM_STATUS_HPP) + create_stream_list_item(), +#endif // OPEN_DRONE_ID_ARM_STATUS_HPP #if defined(ESC_INFO_HPP) create_stream_list_item(), #endif // ESC_INFO_HPP diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 58d2c044ccd0..9c53b16f3693 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -280,6 +280,18 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_statustext(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: @@ -3084,6 +3096,64 @@ MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t _gimbal_device_attitude_status_pub.publish(gimbal_attitude_status); } +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(); + 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(); + 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(); + 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 b95bdca59a72..39fe7c33f9c7 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -87,6 +87,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -182,6 +185,9 @@ class MavlinkReceiver : public ModuleParams void handle_message_obstacle_distance(mavlink_message_t *msg); void handle_message_odometry(mavlink_message_t *msg); void handle_message_onboard_computer_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); @@ -312,6 +318,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 _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)}; diff --git a/src/modules/mavlink/open_drone_id_translations.cpp b/src/modules/mavlink/open_drone_id_translations.cpp new file mode 100644 index 000000000000..854e7623043d --- /dev/null +++ b/src/modules/mavlink/open_drone_id_translations.cpp @@ -0,0 +1,255 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "open_drone_id_translations.hpp" +#include +#include + +using namespace time_literals; + + +namespace open_drone_id_translations +{ + +MAV_ODID_UA_TYPE odidTypeForMavType(uint8_t system_type) +{ + switch (system_type) { + case MAV_TYPE_GENERIC: return MAV_ODID_UA_TYPE_OTHER; + + case MAV_TYPE_FIXED_WING: return MAV_ODID_UA_TYPE_AEROPLANE; + + case MAV_TYPE_QUADROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR; + + case MAV_TYPE_COAXIAL: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR; + + case MAV_TYPE_HELICOPTER: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR; + + case MAV_TYPE_ANTENNA_TRACKER: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_GCS: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_AIRSHIP: return MAV_ODID_UA_TYPE_AIRSHIP; + + case MAV_TYPE_FREE_BALLOON: return MAV_ODID_UA_TYPE_FREE_BALLOON; + + case MAV_TYPE_ROCKET: return MAV_ODID_UA_TYPE_ROCKET; + + case MAV_TYPE_GROUND_ROVER: return MAV_ODID_UA_TYPE_GROUND_OBSTACLE; + + case MAV_TYPE_SURFACE_BOAT: return MAV_ODID_UA_TYPE_OTHER; + + case MAV_TYPE_SUBMARINE: return MAV_ODID_UA_TYPE_OTHER; + + case MAV_TYPE_HEXAROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR; + + case MAV_TYPE_OCTOROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR; + + case MAV_TYPE_TRICOPTER: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR; + + case MAV_TYPE_FLAPPING_WING: return MAV_ODID_UA_TYPE_ORNITHOPTER; + + case MAV_TYPE_KITE: return MAV_ODID_UA_TYPE_KITE; + + case MAV_TYPE_ONBOARD_CONTROLLER: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_VTOL_TAILSITTER_DUOROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT; + + case MAV_TYPE_VTOL_TAILSITTER_QUADROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT; + + case MAV_TYPE_VTOL_TILTROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT; + + case MAV_TYPE_VTOL_FIXEDROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT; + + case MAV_TYPE_VTOL_TAILSITTER: return MAV_ODID_UA_TYPE_HYBRID_LIFT; + + case MAV_TYPE_VTOL_TILTWING: return MAV_ODID_UA_TYPE_HYBRID_LIFT; + + case MAV_TYPE_VTOL_RESERVED5: return MAV_ODID_UA_TYPE_HYBRID_LIFT; + + case MAV_TYPE_GIMBAL: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_ADSB: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_PARAFOIL: return MAV_ODID_UA_TYPE_FREE_FALL_PARACHUTE; + + case MAV_TYPE_DODECAROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR; + + case MAV_TYPE_CAMERA: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_CHARGING_STATION: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_FLARM: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_SERVO: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_ODID: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_DECAROTOR: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_BATTERY: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_PARACHUTE: return MAV_ODID_UA_TYPE_FREE_FALL_PARACHUTE; + + case MAV_TYPE_LOG: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_OSD: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_IMU: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_GPS: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_WINCH: return MAV_ODID_UA_TYPE_NONE; + + default: return MAV_ODID_UA_TYPE_OTHER; + } +} + +MAV_ODID_SPEED_ACC odidSpeedAccForVariance(float s_variance_m_s) +{ + // TODO: should this be stddev, so square root of variance? + // speed_accuracy + if (s_variance_m_s < 0.3f) { + return MAV_ODID_SPEED_ACC_0_3_METERS_PER_SECOND; + + } else if (s_variance_m_s < 1.f) { + return MAV_ODID_SPEED_ACC_1_METERS_PER_SECOND; + + } else if (s_variance_m_s < 3.f) { + return MAV_ODID_SPEED_ACC_3_METERS_PER_SECOND; + + } else if (s_variance_m_s < 10.f) { + return MAV_ODID_SPEED_ACC_10_METERS_PER_SECOND; + + } else { + return MAV_ODID_SPEED_ACC_UNKNOWN; + } +} + +MAV_ODID_HOR_ACC odidHorAccForEph(float eph) +{ + if (eph < 1.f) { + return MAV_ODID_HOR_ACC_1_METER; + + } else if (eph < 3.f) { + return MAV_ODID_HOR_ACC_3_METER; + + } else if (eph < 10.f) { + return MAV_ODID_HOR_ACC_10_METER; + + } else if (eph < 30.f) { + return MAV_ODID_HOR_ACC_30_METER; + + } else { + return MAV_ODID_HOR_ACC_UNKNOWN; + } +} + +MAV_ODID_VER_ACC odidVerAccForEpv(float epv) +{ + if (epv < 1.f) { + return MAV_ODID_VER_ACC_1_METER; + + } else if (epv < 3.f) { + return MAV_ODID_VER_ACC_3_METER; + + } else if (epv < 10.f) { + return MAV_ODID_VER_ACC_10_METER; + + } else if (epv < 25.f) { + return MAV_ODID_VER_ACC_25_METER; + + } else if (epv < 45.f) { + return MAV_ODID_VER_ACC_45_METER; + + } else if (epv < 150.f) { + return MAV_ODID_VER_ACC_150_METER; + + } else { + return MAV_ODID_VER_ACC_UNKNOWN; + } +} + + +MAV_ODID_TIME_ACC odidTimeForElapsed(uint64_t elapsed) +{ + if (elapsed < 100_ms) { + return MAV_ODID_TIME_ACC_0_1_SECOND; + + } else if (elapsed < 200_ms) { + return MAV_ODID_TIME_ACC_0_2_SECOND; + + } else if (elapsed < 300_ms) { + return MAV_ODID_TIME_ACC_0_3_SECOND; + + } else if (elapsed < 400_ms) { + return MAV_ODID_TIME_ACC_0_4_SECOND; + + } else if (elapsed < 500_ms) { + return MAV_ODID_TIME_ACC_0_5_SECOND; + + } else if (elapsed < 600_ms) { + return MAV_ODID_TIME_ACC_0_6_SECOND; + + } else if (elapsed < 700_ms) { + return MAV_ODID_TIME_ACC_0_7_SECOND; + + } else if (elapsed < 800_ms) { + return MAV_ODID_TIME_ACC_0_8_SECOND; + + } else if (elapsed < 900_ms) { + return MAV_ODID_TIME_ACC_0_9_SECOND; + + } else if (elapsed < 1000_ms) { + return MAV_ODID_TIME_ACC_1_0_SECOND; + + } else if (elapsed < 1100_ms) { + return MAV_ODID_TIME_ACC_1_1_SECOND; + + } else if (elapsed < 1200_ms) { + return MAV_ODID_TIME_ACC_1_2_SECOND; + + } else if (elapsed < 1300_ms) { + return MAV_ODID_TIME_ACC_1_3_SECOND; + + } else if (elapsed < 1400_ms) { + return MAV_ODID_TIME_ACC_1_4_SECOND; + + } else if (elapsed < 1500_ms) { + return MAV_ODID_TIME_ACC_1_5_SECOND; + + } else { + return MAV_ODID_TIME_ACC_UNKNOWN; + } +} + +} // open_drone_id_translations diff --git a/src/modules/mavlink/open_drone_id_translations.hpp b/src/modules/mavlink/open_drone_id_translations.hpp new file mode 100644 index 000000000000..4632ed1e14e6 --- /dev/null +++ b/src/modules/mavlink/open_drone_id_translations.hpp @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +namespace open_drone_id_translations +{ + +MAV_ODID_UA_TYPE odidTypeForMavType(uint8_t system_type); + +MAV_ODID_SPEED_ACC odidSpeedAccForVariance(float s_variance_m_s); + +MAV_ODID_HOR_ACC odidHorAccForEph(float eph); + +MAV_ODID_VER_ACC odidVerAccForEpv(float epv); + +MAV_ODID_TIME_ACC odidTimeForElapsed(uint64_t elapsed); + +} // open_drone_id_translations diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_ARM_STATUS.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_ARM_STATUS.hpp new file mode 100644 index 000000000000..691ea3645533 --- /dev/null +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_ARM_STATUS.hpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef OPEN_DRONE_ID_ARM_STATUS_HPP +#define OPEN_DRONE_ID_ARM_STATUS_HPP + +#include + +class MavlinkStreamOpenDroneIdArmStatus : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamOpenDroneIdArmStatus(mavlink); + } + + static constexpr const char *get_name_static() + { + return "OPEN_DRONE_ID_ARM_STATUS"; + } + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS; + } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _open_drone_id_arm_status_sub.advertised() + ? MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN + + MAVLINK_NUM_NON_PAYLOAD_BYTES + : 0; + } + +private: + explicit MavlinkStreamOpenDroneIdArmStatus(Mavlink *mavlink) + : MavlinkStream(mavlink) {} + + uORB::Subscription _open_drone_id_arm_status_sub{ORB_ID(open_drone_id_arm_status)}; + + bool send() override + { + open_drone_id_arm_status_s drone_id_arm; + + if (_open_drone_id_arm_status_sub.update(&drone_id_arm)) { + + mavlink_open_drone_id_arm_status_t msg{}; + + msg.status = drone_id_arm.status; + + for (uint8_t i = 0; i < sizeof(drone_id_arm.error); ++i) { + + msg.error[i] = drone_id_arm.error[i]; + + } + + mavlink_msg_open_drone_id_arm_status_send_struct(_mavlink->get_channel(), + &msg); + + return true; + } + + return false; + } +}; + +#endif // OPEN_DRONE_ID_ARM_STATUS_HPP diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp index d7869dcbb6c9..b51d3e79d148 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp @@ -35,6 +35,7 @@ #define OPEN_DRONE_ID_BASIC_ID_HPP #include +#include class MavlinkStreamOpenDroneIdBasicId : public MavlinkStream { @@ -57,98 +58,6 @@ class MavlinkStreamOpenDroneIdBasicId : public MavlinkStream uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - MAV_ODID_UA_TYPE odidTypeForMavType(uint8_t system_type) - { - switch (system_type) { - case MAV_TYPE_GENERIC: return MAV_ODID_UA_TYPE_OTHER; - - case MAV_TYPE_FIXED_WING: return MAV_ODID_UA_TYPE_AEROPLANE; - - case MAV_TYPE_QUADROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR; - - case MAV_TYPE_COAXIAL: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR; - - case MAV_TYPE_HELICOPTER: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR; - - case MAV_TYPE_ANTENNA_TRACKER: return MAV_ODID_UA_TYPE_NONE; - - case MAV_TYPE_GCS: return MAV_ODID_UA_TYPE_NONE; - - case MAV_TYPE_AIRSHIP: return MAV_ODID_UA_TYPE_AIRSHIP; - - case MAV_TYPE_FREE_BALLOON: return MAV_ODID_UA_TYPE_FREE_BALLOON; - - case MAV_TYPE_ROCKET: return MAV_ODID_UA_TYPE_ROCKET; - - case MAV_TYPE_GROUND_ROVER: return MAV_ODID_UA_TYPE_GROUND_OBSTACLE; - - case MAV_TYPE_SURFACE_BOAT: return MAV_ODID_UA_TYPE_OTHER; - - case MAV_TYPE_SUBMARINE: return MAV_ODID_UA_TYPE_OTHER; - - case MAV_TYPE_HEXAROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR; - - case MAV_TYPE_OCTOROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR; - - case MAV_TYPE_TRICOPTER: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR; - - case MAV_TYPE_FLAPPING_WING: return MAV_ODID_UA_TYPE_ORNITHOPTER; - - case MAV_TYPE_KITE: return MAV_ODID_UA_TYPE_KITE; - - case MAV_TYPE_ONBOARD_CONTROLLER: return MAV_ODID_UA_TYPE_NONE; - - case MAV_TYPE_VTOL_TAILSITTER_DUOROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT; - - case MAV_TYPE_VTOL_TAILSITTER_QUADROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT; - - case MAV_TYPE_VTOL_TILTROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT; - - case MAV_TYPE_VTOL_FIXEDROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT; - - case MAV_TYPE_VTOL_TAILSITTER: return MAV_ODID_UA_TYPE_HYBRID_LIFT; - - case MAV_TYPE_VTOL_TILTWING: return MAV_ODID_UA_TYPE_HYBRID_LIFT; - - case MAV_TYPE_VTOL_RESERVED5: return MAV_ODID_UA_TYPE_HYBRID_LIFT; - - case MAV_TYPE_GIMBAL: return MAV_ODID_UA_TYPE_NONE; - - case MAV_TYPE_ADSB: return MAV_ODID_UA_TYPE_NONE; - - case MAV_TYPE_PARAFOIL: return MAV_ODID_UA_TYPE_FREE_FALL_PARACHUTE; - - case MAV_TYPE_DODECAROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR; - - case MAV_TYPE_CAMERA: return MAV_ODID_UA_TYPE_NONE; - - case MAV_TYPE_CHARGING_STATION: return MAV_ODID_UA_TYPE_NONE; - - case MAV_TYPE_FLARM: return MAV_ODID_UA_TYPE_NONE; - - case MAV_TYPE_SERVO: return MAV_ODID_UA_TYPE_NONE; - - case MAV_TYPE_ODID: return MAV_ODID_UA_TYPE_NONE; - - case MAV_TYPE_DECAROTOR: return MAV_ODID_UA_TYPE_NONE; - - case MAV_TYPE_BATTERY: return MAV_ODID_UA_TYPE_NONE; - - case MAV_TYPE_PARACHUTE: return MAV_ODID_UA_TYPE_FREE_FALL_PARACHUTE; - - case MAV_TYPE_LOG: return MAV_ODID_UA_TYPE_NONE; - - case MAV_TYPE_OSD: return MAV_ODID_UA_TYPE_NONE; - - case MAV_TYPE_IMU: return MAV_ODID_UA_TYPE_NONE; - - case MAV_TYPE_GPS: return MAV_ODID_UA_TYPE_NONE; - - case MAV_TYPE_WINCH: return MAV_ODID_UA_TYPE_NONE; - - default: return MAV_ODID_UA_TYPE_OTHER; - } - } bool send() override @@ -167,7 +76,7 @@ class MavlinkStreamOpenDroneIdBasicId : public MavlinkStream msg.id_type = MAV_ODID_ID_TYPE_SERIAL_NUMBER; // ua_type: MAV_ODID_UA_TYPE - msg.ua_type = odidTypeForMavType(vehicle_status.system_type); + msg.ua_type = open_drone_id_translations::odidTypeForMavType(vehicle_status.system_type); // uas_id: UAS (Unmanned Aircraft System) ID following the format specified by id_type // TODO: MAV_ODID_ID_TYPE_SERIAL_NUMBER needs to be ANSI/CTA-2063 format diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp index 3a519c5e909a..1eba926a3531 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp @@ -144,22 +144,7 @@ class MavlinkStreamOpenDroneIdLocation : public MavlinkStream const int speed_vertical_cm_s = roundf(-vel_ned(2) * 100.f); msg.speed_vertical = math::constrain(speed_vertical_cm_s, -6200, 6200); - // speed_accuracy - if (vehicle_gps_position.s_variance_m_s < 0.3f) { - msg.speed_accuracy = MAV_ODID_SPEED_ACC_0_3_METERS_PER_SECOND; - - } else if (vehicle_gps_position.s_variance_m_s < 1.f) { - msg.speed_accuracy = MAV_ODID_SPEED_ACC_1_METERS_PER_SECOND; - - } else if (vehicle_gps_position.s_variance_m_s < 3.f) { - msg.speed_accuracy = MAV_ODID_SPEED_ACC_3_METERS_PER_SECOND; - - } else if (vehicle_gps_position.s_variance_m_s < 10.f) { - msg.speed_accuracy = MAV_ODID_SPEED_ACC_10_METERS_PER_SECOND; - - } else { - msg.speed_accuracy = MAV_ODID_SPEED_ACC_UNKNOWN; - } + msg.speed_accuracy = open_drone_id_translations::odidSpeedAccForVariance(vehicle_gps_position.s_variance_m_s); updated = true; } @@ -173,45 +158,9 @@ class MavlinkStreamOpenDroneIdLocation : public MavlinkStream msg.altitude_geodetic = static_cast(round(vehicle_gps_position.altitude_msl_m)); // [m] } - // horizontal_accuracy - if (vehicle_gps_position.eph < 1.f) { - msg.horizontal_accuracy = MAV_ODID_HOR_ACC_1_METER; - - } else if (vehicle_gps_position.eph < 3.f) { - msg.horizontal_accuracy = MAV_ODID_HOR_ACC_3_METER; - - } else if (vehicle_gps_position.eph < 10.f) { - msg.horizontal_accuracy = MAV_ODID_HOR_ACC_10_METER; - - } else if (vehicle_gps_position.eph < 30.f) { - msg.horizontal_accuracy = MAV_ODID_HOR_ACC_30_METER; - - } else { - msg.horizontal_accuracy = MAV_ODID_HOR_ACC_UNKNOWN; - } - - // vertical_accuracy - if (vehicle_gps_position.epv < 1.f) { - msg.vertical_accuracy = MAV_ODID_VER_ACC_1_METER; - - } else if (vehicle_gps_position.epv < 3.f) { - msg.vertical_accuracy = MAV_ODID_VER_ACC_3_METER; - - } else if (vehicle_gps_position.epv < 10.f) { - msg.vertical_accuracy = MAV_ODID_VER_ACC_10_METER; - - } else if (vehicle_gps_position.epv < 25.f) { - msg.vertical_accuracy = MAV_ODID_VER_ACC_25_METER; + msg.horizontal_accuracy = open_drone_id_translations::odidHorAccForEph(vehicle_gps_position.eph); - } else if (vehicle_gps_position.epv < 45.f) { - msg.vertical_accuracy = MAV_ODID_VER_ACC_45_METER; - - } else if (vehicle_gps_position.epv < 150.f) { - msg.vertical_accuracy = MAV_ODID_VER_ACC_150_METER; - - } else { - msg.vertical_accuracy = MAV_ODID_VER_ACC_UNKNOWN; - } + msg.vertical_accuracy = open_drone_id_translations::odidVerAccForEpv(vehicle_gps_position.epv); updated = true; } @@ -221,9 +170,8 @@ class MavlinkStreamOpenDroneIdLocation : public MavlinkStream uint64_t utc_time_msec = vehicle_gps_position.time_utc_usec / 1000; msg.timestamp = ((float)(utc_time_msec % (60 * 60 * 1000))) / 1000; - if (hrt_elapsed_time(&vehicle_gps_position.timestamp) < 1_s) { - msg.timestamp_accuracy = MAV_ODID_TIME_ACC_1_0_SECOND; // TODO - } + msg.timestamp_accuracy = open_drone_id_translations::odidTimeForElapsed(hrt_elapsed_time( + &vehicle_gps_position.timestamp)); updated = true; } @@ -236,7 +184,7 @@ class MavlinkStreamOpenDroneIdLocation : public MavlinkStream if (_vehicle_air_data_sub.copy(&vehicle_air_data) && (hrt_elapsed_time(&vehicle_air_data.timestamp) < 10_s)) { msg.altitude_barometric = vehicle_air_data.baro_alt_meter; - msg.barometer_accuracy = MAV_ODID_VER_ACC_150_METER; // TODO + msg.barometer_accuracy = MAV_ODID_VER_ACC_UNKNOWN; // We just don't without calibration. updated = true; } }