From fb9c3f321bc1b465af9e1adef7d458757c9be6f7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 10 Jul 2024 13:56:19 +1200 Subject: [PATCH] uavcan: implement OpenDroneID Location --- src/drivers/uavcan/remoteid.cpp | 157 +++++++++++++++++++++++++++++++- src/drivers/uavcan/remoteid.hpp | 24 +++-- 2 files changed, 168 insertions(+), 13 deletions(-) diff --git a/src/drivers/uavcan/remoteid.cpp b/src/drivers/uavcan/remoteid.cpp index 7c8bc7f822cc..987f89a97939 100644 --- a/src/drivers/uavcan/remoteid.cpp +++ b/src/drivers/uavcan/remoteid.cpp @@ -33,12 +33,17 @@ #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_basicid(node), + _uavcan_pub_remoteid_location(node) { } @@ -52,15 +57,19 @@ int UavcanRemoteIDController::init() void UavcanRemoteIDController::periodic_update(const uavcan::TimerEvent &) { - if (!_vehicle_status_sub.update()) { - return; - } + _vehicle_status.update(); + send_basic_id(); + send_location(); +} + +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_sub.get().system_type)); + _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 @@ -71,3 +80,141 @@ void UavcanRemoteIDController::periodic_update(const uavcan::TimerEvent &) _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); + } +} diff --git a/src/drivers/uavcan/remoteid.hpp b/src/drivers/uavcan/remoteid.hpp index 839a2f8cf2d5..8657ae476242 100644 --- a/src/drivers/uavcan/remoteid.hpp +++ b/src/drivers/uavcan/remoteid.hpp @@ -33,11 +33,17 @@ #pragma once +#include #include +#include +#include +#include +#include #include #include #include +#include #include @@ -58,16 +64,18 @@ class UavcanRemoteIDController : public ModuleParams void periodic_update(const uavcan::TimerEvent &); + void send_basic_id(); + void send_location(); + uavcan::INode &_node; - uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; + 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)}; uavcan::Publisher _uavcan_pub_remoteid_basicid; - - //DEFINE_PARAMETERS( - // (ParamInt) _param_mode_anti_col, - // (ParamInt) _param_mode_strobe, - // (ParamInt) _param_mode_nav, - // (ParamInt) _param_mode_land - //) + uavcan::Publisher _uavcan_pub_remoteid_location; };