diff --git a/rosflight_io/include/rosflight_io/rosflight_io.hpp b/rosflight_io/include/rosflight_io/rosflight_io.hpp index e3343ec..4e17336 100644 --- a/rosflight_io/include/rosflight_io/rosflight_io.hpp +++ b/rosflight_io/include/rosflight_io/rosflight_io.hpp @@ -365,7 +365,6 @@ class ROSflightIO : public rclcpp::Node, * @brief "command" topic subscription callback. * * This function is called anytime rosflight_io receives a message on the "command" topic. - * It saturates the commands and sends them over MAVLink to the firmware. * * @param msg Populated ROSflight Command message. */ @@ -577,21 +576,6 @@ class ROSflightIO : public rclcpp::Node, */ rclcpp::Time fcu_time_to_ros_time(std::chrono::nanoseconds fcu_time); - template - /** - * @brief Saturates values to not exceed specified range. - * @tparam T Object type to perform saturation with. Needs to have greater than and less than - * comparators. - * @param value Value to saturate. - * @param min Lower bound of saturation range. - * @param max Upper bound of saturation range. - * @return Saturated value. - */ - inline T saturate(T value, T min, T max) - { - return value < min ? min : (value > max ? max : value); - } - /// "command" ROS topic subscription. rclcpp::Subscription::SharedPtr command_sub_; /// "aux_command" ROS topic subscription. diff --git a/rosflight_io/src/rosflight_io.cpp b/rosflight_io/src/rosflight_io.cpp index 937726d..83ce6f7 100644 --- a/rosflight_io/src/rosflight_io.cpp +++ b/rosflight_io/src/rosflight_io.cpp @@ -957,20 +957,6 @@ void ROSflightIO::commandCallback(const rosflight_msgs::msg::Command::ConstShare float z = msg->z; float F = msg->f; - switch (mode) { - case MODE_PASS_THROUGH: - x = saturate(x, -1.0f, 1.0f); - y = saturate(y, -1.0f, 1.0f); - z = saturate(z, -1.0f, 1.0f); - F = saturate(F, 0.0f, 1.0f); - break; - case MODE_ROLL_PITCH_YAWRATE_THROTTLE: - F = saturate(F, 0.0f, 1.0f); - break; - default: - break; - } - mavlink_message_t mavlink_msg; mavlink_msg_offboard_control_pack(1, 50, &mavlink_msg, mode, ignore, x, y, z, F); mavrosflight_->comm.send_message(mavlink_msg);