Skip to content

{Sponsored by Intelliterra} OPEN_DRONE_ID_SYSTEM, OPEN_DRONE_ID_SELF_ID, and OPEN_DRONE_ID_OPERATOR_ID values getting cached within PX4 #1

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
dirksavage88 merged 8 commits into from
Jul 7, 2023
Merged

{Sponsored by Intelliterra} OPEN_DRONE_ID_SYSTEM, OPEN_DRONE_ID_SELF_ID, and OPEN_DRONE_ID_OPERATOR_ID values getting cached within PX4 #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
7259a21
Operator id added to mavlink receiver
jnomikos Jul 3, 2023
6bb05af
System id added to mavlink receiver
jnomikos Jul 3, 2023
bb184fb
Self id added to mavlink receiver
jnomikos Jul 3, 2023
ae4551b
Self_ID, calls handler when mavlink message recieved
jnomikos Jul 6, 2023
c6996d2
Operator and self id now subscribed to uORB messages
jnomikos Jul 6, 2023
68309e0
changed .copy to .update and made it apart of an if check
jnomikos Jul 6, 2023
c75a42b
OPEN_DRONE_ID_SYSTEM now gets info from uORB if available
jnomikos Jul 6, 2023
17f706b
operator_latitude, operator_longitude, should be int32
jnomikos Jul 7, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions msg/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
6 changes: 6 additions & 0 deletions msg/OpenDroneIdOperatorId.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
6 changes: 6 additions & 0 deletions msg/OpenDroneIdSelfId.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
uint64 timestamp
uint8 target_system
uint8 target_component
uint8[20] id_or_mac
uint8 description_type
char[23] description
15 changes: 15 additions & 0 deletions msg/OpenDroneIdSystem.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
78 changes: 78 additions & 0 deletions src/modules/mavlink/mavlink_receiver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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()
{
Expand Down
9 changes: 9 additions & 0 deletions src/modules/mavlink/mavlink_receiver.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,9 @@
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/onboard_computer_status.h>
#include <uORB/topics/open_drone_id_arm_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>
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -308,6 +314,9 @@ class MavlinkReceiver : public ModuleParams
uORB::Publication<offboard_control_mode_s> _offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
uORB::Publication<onboard_computer_status_s> _onboard_computer_status_pub{ORB_ID(onboard_computer_status)};
uORB::Publication<open_drone_id_arm_status_s> _open_drone_id_arm_status_pub{ORB_ID(open_drone_id_arm_status)};
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)};
Expand Down
33 changes: 21 additions & 12 deletions src/modules/mavlink/streams/OPEN_DRONE_ID_OPERATOR_ID.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
#define OPEN_DRONE_ID_OPERATOR_ID_HPP

#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/open_drone_id_operator_id.h>

class MavlinkStreamOpenDroneIdOperatorId : public MavlinkStream
{
Expand Down Expand Up @@ -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;
}
};

Expand Down
33 changes: 21 additions & 12 deletions src/modules/mavlink/streams/OPEN_DRONE_ID_SELF_ID.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
#define OPEN_DRONE_ID_SELF_ID_HPP

#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/open_drone_id_self_id.h>

class MavlinkStreamOpenDroneIdSelfId : public MavlinkStream
{
Expand Down Expand Up @@ -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;
}
};

Expand Down
36 changes: 34 additions & 2 deletions src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@

#include <uORB/topics/home_position.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/open_drone_id_system.h>

class MavlinkStreamOpenDroneIdSystem : public MavlinkStream
{
Expand Down Expand Up @@ -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;
Expand All @@ -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;
}
};
Expand Down