fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Close fds when not needed

Browse Source 
File descriptors get closed when not needed by offboard mode
to allow position and attitude controllers to advertise and publish.
This commit is contained in:
t0ni0 2014-06-05 04:03:40 -04:00
parent af56879fdd
commit 2a79a9a4e4
3 changed files with 53 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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;

8
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -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 */

8
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -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();