Unable to set speed more that 9.5 m/s in SITL

Problem Statement

  • Sending 30 m/s velocity to an IRIS drone on SITL simulation in OFFBOARD mode but the ground speed is still shown 9.5 m/s in QGroundControl.
  • Using Mavros, I am sending 30m/s to the setpoint_raw/local topic.

Setup

  1. Simulation setup: SITL - PX4 Gazebo
  2. Firmware: 1.10.1
  3. Mode: OFFBOARD
  4. MAVROS
  5. MPC_XY_VEL_MAX: 20m/s

Steps to reproduce

  1. Start SITL simulation on gazebo.
  2. Run the script shown below that sends 30m/s to the PositionTarget message type.

More information

Speed shown in QGroundControl:

Speed published:

Code

/**
 * @file offb_node.cpp
 * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro
 * Flight Stack and tested in Gazebo SITL
 */

#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <ros/ros.h>

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr &msg) { current_state = *msg; }

int main(int argc, char **argv)
{
  ros::init(argc, argv, "offb_node");
  ros::NodeHandle nh;

  ros::Subscriber state_sub =
      nh.subscribe<mavros_msgs::State>("robot100/sitl/state", 10, state_cb);
  ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>(
      "robot100/sitl/setpoint_position/local", 10);
  ros::Publisher local_vel_pub =
      nh.advertise<mavros_msgs::PositionTarget>("robot100/sitl/setpoint_raw/local", 10);
  ros::ServiceClient arming_client =
      nh.serviceClient<mavros_msgs::CommandBool>("robot100/sitl/cmd/arming");
  ros::ServiceClient set_mode_client =
      nh.serviceClient<mavros_msgs::SetMode>("robot100/sitl/set_mode");

  // the setpoint publishing rate MUST be faster than 2Hz
  ros::Rate rate(20.0);

  // wait for FCU connection
  while (ros::ok() && !current_state.connected)
  {
    ros::spinOnce();
    rate.sleep();
  }

  geometry_msgs::PoseStamped pose;
  pose.pose.position.x = 0;
  pose.pose.position.y = 0;
  pose.pose.position.z = 2;

  mavros_msgs::PositionTarget setpoint_msg;
  setpoint_msg.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_NED;
  setpoint_msg.type_mask = 0b0000011111000111;
  setpoint_msg.position.x = 0;
  setpoint_msg.position.y = 0;
  setpoint_msg.position.z = 0;
  setpoint_msg.velocity.x = 30;
  setpoint_msg.velocity.y = 0;
  setpoint_msg.velocity.z = 0;
  setpoint_msg.yaw_rate = 0;

  // send a few setpoints before starting
  for (int i = 100; ros::ok() && i > 0; --i)
  {
    local_pos_pub.publish(pose);
    ros::spinOnce();
    rate.sleep();
  }

  mavros_msgs::SetMode offb_set_mode;
  offb_set_mode.request.custom_mode = "OFFBOARD";

  mavros_msgs::CommandBool arm_cmd;
  arm_cmd.request.value = true;

  ros::Time last_request = ros::Time::now();
  bool takeoff;
  takeoff = false;

  while (ros::ok())
  {
    if (current_state.mode != "OFFBOARD" &&
        (ros::Time::now() - last_request > ros::Duration(5.0)))
    {
      if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
      {
        ROS_INFO("Offboard enabled");
      }
      last_request = ros::Time::now();
    }
    else
    {
      if (!current_state.armed &&
          (ros::Time::now() - last_request > ros::Duration(5.0)))
      {
        if (arming_client.call(arm_cmd) && arm_cmd.response.success)
        {
          ROS_INFO("Vehicle armed");
        }
        last_request = ros::Time::now();
      }
    }

    if (!takeoff)
    {
      for (int i = 0; i < 10; i++)
      {
        local_pos_pub.publish(pose);
      }
      takeoff = true;
    }

    setpoint_msg.header.stamp = ros::Time::now();
    local_vel_pub.publish(setpoint_msg);

    ros::spinOnce();
    rate.sleep();
  }

  return 0;
}