fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-28 05:54:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Removed publications closing

Browse Source 
This is an attempt to correct the offboard setpoints being passed on as "NaN" values
This commit is contained in:
t0ni0 2014-06-17 13:21:20 -04:00
parent 6183390547
commit e078ef992f
3 changed files with 3 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

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

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

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