Removed publications closing

This is an attempt to correct the offboard setpoints being passed on as "NaN" values

1 files changed, 0 insertions, 59 deletions

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index c95ee880c..5528aca5e 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -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;