aboutsummaryrefslogtreecommitdiff
path: root/src/modules
diff options
context:
space:
mode:
authort0ni0 <azntonio789@gmail.com>2014-06-17 13:21:20 -0400
committert0ni0 <azntonio789@gmail.com>2014-06-17 13:30:45 -0400
commite078ef992fc21d86cbd09db89c25332273840b22 (patch)
treeec022c5fa66776a595a073ee06267cc253571021 /src/modules
parent61833905472465ebab615ecdd567b78eb1628d97 (diff)
downloadpx4-firmware-e078ef992fc21d86cbd09db89c25332273840b22.tar.gz
px4-firmware-e078ef992fc21d86cbd09db89c25332273840b22.tar.bz2
px4-firmware-e078ef992fc21d86cbd09db89c25332273840b22.zip
Removed publications closing
This is an attempt to correct the offboard setpoints being passed on as "NaN" values
Diffstat (limited to 'src/modules')
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp59
-rw-r--r--src/modules/mc_att_control/mc_att_control_main.cpp8
-rw-r--r--src/modules/mc_pos_control/mc_pos_control_main.cpp20
3 files changed, 3 insertions, 84 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index c95ee880c..5528aca5e 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -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;
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 26c0c386f..20e016da3 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -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 */
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 35db05499..41fb20108 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -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 */