Close fds when not needed

File descriptors get closed when not needed by offboard mode
to allow position and attitude controllers to advertise and publish.

1 files changed, 7 insertions, 1 deletions

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 87a8385d3..4ee78516f 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -734,10 +734,16 @@ 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 {
+			} else if (!_control_mode.flag_control_offboard_enabled) {
 				_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 */
 				R.identity();