From 2a79a9a4e4ebae2e85dab64ae98ff2a17e421ea2 Mon Sep 17 00:00:00 2001
From: t0ni0 <azntonio789@gmail.com>
Date: Thu, 5 Jun 2014 04:03:40 -0400
Subject: [PATCH] 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.
---
 src/modules/mavlink/mavlink_receiver.cpp      | 39 +++++++++++++++++++
 .../mc_att_control/mc_att_control_main.cpp    |  8 +++-
 .../mc_pos_control/mc_pos_control_main.cpp    |  8 +++-
 3 files changed, 53 insertions(+), 2 deletions(-)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 2157ae00bb..3b3423a4b6 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -422,6 +422,17 @@ 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 (_local_pos_sp_pub < 0) {
 						_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp_pub);
 
@@ -444,6 +455,12 @@ 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);
 
@@ -993,6 +1010,28 @@ 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 (_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;
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 20e016da34..26c0c386f0 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -584,9 +584,15 @@ MulticopterAttitudeControl::control_attitude(float dt)
 		if (_att_sp_pub > 0) {
 			orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp);
 
-		} else {
+		} else if (!_v_control_mode.flag_control_offboard_enabled) {
 			_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 */
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 87a8385d33..4ee78516fa 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();