Removed publications closing

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

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;

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -584,15 +584,9 @@ MulticopterAttitudeControl::control_attitude(float dt)
 		if (_att_sp_pub > 0) {
 			orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp);
 
-		} else if (!_v_control_mode.flag_control_offboard_enabled) {
+		} else {
 			_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp);
 		}
-
-		/* Close fd to let offboard att sp be advertised in mavlink receiver*/
-		if (_v_control_mode.flag_control_offboard_enabled && _att_sp_pub > 0) {
-			close(_att_sp_pub);
-			_att_sp_pub = -1;
-		}
 	}
 
 	/* rotation matrix for current state */

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -733,15 +733,10 @@ MulticopterPositionControl::task_main()
 			if (_local_pos_sp_pub > 0) {
 				orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
 
-			} else if (!_control_mode.flag_control_offboard_enabled) {
+			} else {
 				_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
 			}
 
-			/* Close fd to let offboard pos sp be advertised in mavlink receiver*/
-			if (_control_mode.flag_control_offboard_enabled && _local_pos_sp_pub > 0) {
-				close(_local_pos_sp_pub);
-				_local_pos_sp_pub = -1;
-			}
 
 			if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
 				/* idle state, don't run controller and set zero thrust */
@@ -822,16 +817,10 @@ MulticopterPositionControl::task_main()
 				if (_global_vel_sp_pub > 0) {
 					orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
 
-				} else if (!_control_mode.flag_control_offboard_enabled) {
+				} else {
 					_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
 				}
 
-				/* Close fd to let offboard vel sp be advertised in mavlink receiver */
-				if (_control_mode.flag_control_offboard_enabled && _global_vel_sp_pub > 0) {
-					close(_global_vel_sp_pub);
-					_global_vel_sp_pub = -1;
-				}
-
 				if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) {
 					/* reset integrals if needed */
 					if (_control_mode.flag_control_climb_rate_enabled) {
@@ -1099,11 +1088,6 @@ MulticopterPositionControl::task_main()
 			reset_int_z = true;
 			reset_int_xy = true;
 
-			/* Close att_sp pub to allow offboard mode or att controller to advertise */
-			if (_att_sp_pub > 0) {
-				close(_att_sp_pub);
-				_att_sp_pub = -1;
-			}
 		}
 
 		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */