Merge pull request #188 from rosflight/187-rosflight_io-node-saturate…

…s-passthrough-input

removed saturation from the rosflight_io node

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<class T>
-  /**
-   * @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<rosflight_msgs::msg::Command>::SharedPtr command_sub_;
   /// "aux_command" ROS topic subscription.

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