diff options
Diffstat (limited to 'src/modules/mavlink/mavlink_receiver.cpp')
| -rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 39 |
1 files changed, 39 insertions, 0 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 2157ae00b..3b3423a4b 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; |