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

Removed publications closing

Browse Source 
This is an attempt to correct the offboard setpoints being passed on as "NaN" values
This commit is contained in:
t0ni0
2014-06-17 13:30:45 -04:00
parent 6183390547
commit e078ef992f
3 changed files with 3 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
-59
View File
@@ -440,22 +440,6 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
loc_pos_sp.yaw = offboard_control_sp.p3;
loc_pos_sp.z = -offboard_control_sp.p4;
/* Close fds to allow position controller to use attitude controller */
if (_att_sp_pub > 0) {
close(_att_sp_pub);
_att_sp_pub = -1;
}
if (_rates_sp_pub > 0) {
close(_rates_sp_pub);
_rates_sp_pub = -1;
}
if (_global_vel_sp_pub > 0) {
close(_global_vel_sp_pub);
_global_vel_sp_pub = -1;
}
if (_local_pos_sp_pub < 0) {
_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp_pub);
@@ -472,16 +456,6 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
global_vel_sp.vy = offboard_control_sp.p2;
global_vel_sp.vz = offboard_control_sp.p3;
if (_att_sp_pub > 0) {
close(_att_sp_pub);
_att_sp_pub = -1;
}
if (_rates_sp_pub > 0) {
close(_rates_sp_pub);
_rates_sp_pub = -1;
}
if (_global_vel_sp_pub < 0) {
_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp_pub);
@@ -501,12 +475,6 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
att_sp.timestamp = hrt_absolute_time();
/* Close fd to allow attitude controller to publish its own rates sp*/
if (_rates_sp_pub > 0) {
close(_rates_sp_pub);
_rates_sp_pub = -1;
}
if (_att_sp_pub < 0) {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
@@ -1056,33 +1024,6 @@ MavlinkReceiver::receive_thread(void *arg)
}
}
}
/* Close unused fds when not in offboard mode anymore */
bool updated;
orb_check(_control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
if (!_control_mode.flag_control_offboard_enabled) {
if (_local_pos_sp_pub > 0) {
close(_local_pos_sp_pub);
_local_pos_sp_pub = -1;
}
if (_global_vel_sp_pub > 0) {
close(_global_vel_sp_pub);
_global_vel_sp_pub = -1;
}
if (_att_sp_pub > 0) {
close(_att_sp_pub);
_att_sp_pub = -1;
}
if (_rates_sp_pub > 0) {
close(_rates_sp_pub);
_rates_sp_pub = -1;
}
}
}
}
return NULL;