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

uavcan: implement OpenDroneID System

Browse Source
This commit is contained in:
Julian Oes 2024-07-11 11:06:00 +12:00
parent de00c23e19
commit d999258171
2 changed files with 40 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -43,7 +43,8 @@ UavcanRemoteIDController::UavcanRemoteIDController(uavcan::INode &node) :
_timer(node),
_node(node),
_uavcan_pub_remoteid_basicid(node),
_uavcan_pub_remoteid_location(node)
_uavcan_pub_remoteid_location(node),
_uavcan_pub_remoteid_system(node)
{
}
@ -61,6 +62,7 @@ void UavcanRemoteIDController::periodic_update(const uavcan::TimerEvent &)
send_basic_id();
send_location();
send_system();
}
void UavcanRemoteIDController::send_basic_id()
@ -218,3 +220,37 @@ void UavcanRemoteIDController::send_location()
_uavcan_pub_remoteid_location.broadcast(msg);
}
}
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);
}
}
}

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

@ -44,6 +44,7 @@
#include <uavcan/uavcan.hpp>
#include <dronecan/remoteid/BasicID.hpp>
#include <dronecan/remoteid/Location.hpp>
#include <dronecan/remoteid/System.hpp>
#include <px4_platform_common/module_params.h>
@ -66,6 +67,7 @@ private:
void send_basic_id();
void send_location();
void send_system();
uavcan::INode &_node;
@ -78,4 +80,5 @@ private:
uavcan::Publisher<dronecan::remoteid::BasicID> _uavcan_pub_remoteid_basicid;
uavcan::Publisher<dronecan::remoteid::Location> _uavcan_pub_remoteid_location;
uavcan::Publisher<dronecan::remoteid::System> _uavcan_pub_remoteid_system;
};