diff options
author | Lorenz Meier <lorenz@px4.io> | 2015-03-02 08:27:23 +0100 |
---|---|---|
committer | Lorenz Meier <lorenz@px4.io> | 2015-03-02 08:27:23 +0100 |
commit | 2aa91a05fe72172c4acaf5689d3cd093a047c42c (patch) | |
tree | 63872240d4b81628d538590d69bd6fa6ee703302 | |
parent | dc678342226e2b200e7c4c030b9d2a456a497d19 (diff) | |
parent | 0dec33526b3d372950733c9a3588d88cc622fc08 (diff) | |
download | px4-firmware-2aa91a05fe72172c4acaf5689d3cd093a047c42c.tar.gz px4-firmware-2aa91a05fe72172c4acaf5689d3cd093a047c42c.tar.bz2 px4-firmware-2aa91a05fe72172c4acaf5689d3cd093a047c42c.zip |
Merge pull request #1857 from PX4/mcposoffboardnoattitudepub
mc pos ctrl (multiplatform): do not publish att sp in offboard && no position/velocity control
-rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 25 | ||||
-rw-r--r-- | src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp | 17 |
2 files changed, 28 insertions, 14 deletions
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 247c8715c..d993692ab 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -761,8 +761,8 @@ void MulticopterPositionControl::control_auto(float dt) orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); //Make sure that the position setpoint is valid - if (!isfinite(_pos_sp_triplet.current.lat) || - !isfinite(_pos_sp_triplet.current.lon) || + if (!isfinite(_pos_sp_triplet.current.lat) || + !isfinite(_pos_sp_triplet.current.lon) || !isfinite(_pos_sp_triplet.current.alt)) { _pos_sp_triplet.current.valid = false; } @@ -1367,7 +1367,7 @@ MulticopterPositionControl::task_main() } else if (yaw_offs > YAW_OFFSET_MAX) { _att_sp.yaw_body = _wrap_pi(_att.yaw + YAW_OFFSET_MAX); - } + } } //Control roll and pitch directly if we no aiding velocity controller is active @@ -1388,15 +1388,22 @@ MulticopterPositionControl::task_main() _att_sp.timestamp = hrt_absolute_time(); } else { - reset_yaw_sp = true; + reset_yaw_sp = true; } - // publish attitude setpoint - if (_att_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + /* publish attitude setpoint + * Do not publish if offboard is enabled but position/velocity control is disabled, + * in this case the attitude setpoint is published by the mavlink app + */ + if (!(_control_mode.flag_control_offboard_enabled && + !(_control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled))) { + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); - } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index 2e14b744f..40268358a 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -1015,12 +1015,19 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti reset_yaw_sp = true; } - /* publish attitude setpoint */ - if (_att_sp_pub != nullptr) { - _att_sp_pub->publish(_att_sp_msg); + /* publish attitude setpoint + * Do not publish if offboard is enabled but position/velocity control is disabled, in this case the attitude setpoint + * is published by the mavlink app + */ + if (!(_control_mode->data().flag_control_offboard_enabled && + !(_control_mode->data().flag_control_position_enabled || + _control_mode->data().flag_control_velocity_enabled))) { + if (_att_sp_pub != nullptr) { + _att_sp_pub->publish(_att_sp_msg); - } else { - _att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>(); + } else { + _att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>(); + } } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ |