VTOL Position and Velocity control

@Rui_Loureiro wrote:

Hello all,

Currently im testing a VTOL integration on mavros using a custom version of gazebo that supports Ardupilot quadplane.

Could anyone tell me if the Position control and speed controls are implemented in quadplane firmware?
The code works for ArduCopter but not for ArduPlane Quadplane aparently…

Here are the portions of code:

void nav_local_pos_setpoint(geometry_msgs::Point point)
{
float ref_quaternion[4];
geometry_msgs::PoseStamped local_setpoint;
ros::Rate rate(4);

local_setpoint.pose.position.x= point.x;
local_setpoint.pose.position.y= point.y;
local_setpoint.pose.position.z= point.z;
mavlink_euler_to_quaternion(0.0, 0.0, 0 , ref_quaternion);
local_setpoint.pose.orientation.w =ref_quaternion[0];
local_setpoint.pose.orientation.x =ref_quaternion[1];
local_setpoint.pose.orientation.y =ref_quaternion[2];
local_setpoint.pose.orientation.z =ref_quaternion[3];

pub_pos_setpoint_local.publish(local_setpoint);

/*while(1)
{
    if( (cp_local.pose.position.x<(point.x+raio)) && (cp_local.pose.position.x>(point.x-raio)) &&
        (cp_local.pose.position.y<(point.y+raio)) && (cp_local.pose.position.y>(point.y-raio)) &&
        (cp_local.pose.position.z<(point.z+raio)) && (cp_local.pose.position.z>(point.z-raio)) )
    {
        break;
    }
    rate.sleep();
    ros::spinOnce();
}*/

}

void nav_local_vel_setpoint(geometry_msgs::Point point, double theta_ref )
{
geometry_msgs::TwistStamped local_setpoint;

local_setpoint.twist.linear.x= point.x;
local_setpoint.twist.linear.y= point.y;
local_setpoint.twist.linear.z= point.z;
local_setpoint.twist.angular.x = 0;
local_setpoint.twist.angular.y = 0;
local_setpoint.twist.angular.z = theta_ref;

pub_vel_setpoint_local.publish(local_setpoint);

}

Thank you for your time

Posts: 1

Participants: 1

Read full topic