fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

remoteid: implement System as sent from GCS

Browse Source 
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.
This commit is contained in:
Julian Oes 2024-07-19 14:14:09 +12:00
parent 7d1d398984
commit cd63cfed3a
6 changed files with 101 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -155,6 +155,7 @@ set(msg_files
OpenDroneIdArmStatus.msg
OpenDroneIdOperatorId.msg
OpenDroneIdSelfId.msg
OpenDroneIdSystem.msg
OrbitStatus.msg
OrbTest.msg
OrbTestLarge.msg

13
msg/OpenDroneIdSystem.msg Normal file
View File

@ -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

77
src/drivers/uavcan/remoteid.cpp
View File

@ -236,34 +236,65 @@ void UavcanRemoteIDController::send_location()
void UavcanRemoteIDController::send_system()
{
sensor_gps_s vehicle_gps_position;
home_position_s home_position;
open_drone_id_system_s system;
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) {
if (_open_drone_id_system.advertised() && _open_drone_id_system.copy(&system)) {
dronecan::remoteid::System msg {};
// Use what ground station sends us.
// 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;
dronecan::remoteid::System msg {};
msg.timestamp = system.timestamp;
// 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;
for (unsigned i = 0; i < sizeof(system.id_or_mac); ++i) {
msg.id_or_mac.push_back(system.id_or_mac[i]);
}
_uavcan_pub_remoteid_system.broadcast(msg);
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);
}
}
}
}

2
src/drivers/uavcan/remoteid.hpp
View File

@ -44,6 +44,7 @@
#include <uORB/topics/open_drone_id_operator_id.h>
#include <uORB/topics/open_drone_id_arm_status.h>
#include <uORB/topics/open_drone_id_self_id.h>
#include <uORB/topics/open_drone_id_system.h>
#include <uavcan/uavcan.hpp>
#include <dronecan/remoteid/BasicID.hpp>
@ -90,6 +91,7 @@ private:
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_s> _open_drone_id_arm_status_pub{ORB_ID(open_drone_id_arm_status)};
uavcan::Publisher<dronecan::remoteid::BasicID> _uavcan_pub_remoteid_basicid;

29
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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:
@ -3125,6 +3129,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()
{

2
src/modules/mavlink/mavlink_receiver.h
View File

@ -89,6 +89,7 @@
#include <uORB/topics/onboard_computer_status.h>
#include <uORB/topics/open_drone_id_operator_id.h>
#include <uORB/topics/open_drone_id_self_id.h>
#include <uORB/topics/open_drone_id_system.h>
#include <uORB/topics/ping.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/radio_status.h>
@ -319,6 +320,7 @@ private:
uORB::Publication<velocity_limits_s> _velocity_limits_pub{ORB_ID(velocity_limits)};
uORB::Publication<open_drone_id_operator_id_s> _open_drone_id_operator_id_pub{ORB_ID(open_drone_id_operator_id)};
uORB::Publication<open_drone_id_self_id_s> _open_drone_id_self_id_pub{ORB_ID(open_drone_id_self_id)};
uORB::Publication<open_drone_id_system_s> _open_drone_id_system_pub{ORB_ID(open_drone_id_system)};
uORB::Publication<generator_status_s> _generator_status_pub{ORB_ID(generator_status)};
uORB::Publication<vehicle_attitude_s> _attitude_pub{ORB_ID(vehicle_attitude)};
uORB::Publication<vehicle_attitude_setpoint_s> _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};