fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 12:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

introduce uORB::PublicationQueued and transition most orb_advertise_queue usage

Browse Source
This commit is contained in:
Daniel Agar
2019-08-04 10:08:09 -04:00
parent 839787568c
commit e69398c09f
62 changed files with 421 additions and 674 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vmount/output_mavlink.cpp
+3 -21
View File
@@ -54,16 +54,9 @@ OutputMavlink::OutputMavlink(const OutputConfig &output_config)
{
}
OutputMavlink::~OutputMavlink()
{
if (_vehicle_command_pub) {
orb_unadvertise(_vehicle_command_pub);
}
}
int OutputMavlink::update(const ControlData *control_data)
{
vehicle_command_s vehicle_command = {};
vehicle_command_s vehicle_command{};
vehicle_command.timestamp = hrt_absolute_time();
vehicle_command.target_system = (uint8_t)_config.mavlink_sys_id;
vehicle_command.target_component = (uint8_t)_config.mavlink_comp_id;
@@ -82,18 +75,7 @@ int OutputMavlink::update(const ControlData *control_data)
vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
}
if (_vehicle_command_pub) {
orb_publish(ORB_ID(vehicle_command), _vehicle_command_pub, &vehicle_command);
} else {
_vehicle_command_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vehicle_command,
vehicle_command_s::ORB_QUEUE_LENGTH);
}
}
if (!_vehicle_command_pub) {
return 0;
_vehicle_command_pub.publish(vehicle_command);
}
_handle_position_update();
@@ -110,7 +92,7 @@ int OutputMavlink::update(const ControlData *control_data)
vehicle_command.param2 = (_angle_outputs[0] + _config.roll_offset) * M_RAD_TO_DEG_F;
vehicle_command.param3 = (_angle_outputs[2] + _config.yaw_offset) * M_RAD_TO_DEG_F;
orb_publish(ORB_ID(vehicle_command), _vehicle_command_pub, &vehicle_command);
_vehicle_command_pub.publish(vehicle_command);
_last_update = t;