# Why does my quadrotor becomes unstable after turning?

Hello everyone, hope you are all having a great time.
I'm trying to write a simple test mavros program where my quadrotor is expected to go in a straight line and then have 180-degree turn and come back and land on where it initially took off.
The issue is that I noticed, if I suddenly change the yaw, it shocks the quadrotor(creates quiet a bit of turbulence) and thus I want to have as smooth of a turn as possible. So for this I thought, maybe hovering in the last position for a couple of iterations would do the trick, however, I noticed after adding this snippet, the drone flies unstably, it shakes badly! and also the last turn before landing, seems to not to take place properly as well!
What am I missing here? any help is greatly appreciated.
Here are two video clips I recorded 1 shows the quadrotor with my loiter_fn, and the other without it.

Clip1 : https://files.fm/f/4ts4b4kdv
Clip2: https://files.fm/f/687gmhj6d
and here is my code :

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <tf/LinearMath/Quaternion.h>

// https://docs.px4.io/master/en/simulation/ros_interface.html

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>("mavros/state", 10, state_cb);
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/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 = 0;

auto to_degree = [](const double &radian_angle)
{
return radian_angle * (180 / 3.14);
};
auto to_radian = [](const double &degree_angle)
{
return (degree_angle * 3.14) / 180;
};

tf::Quaternion qtr;
qtr.setRPY(0, 0, yaw);
pose.pose.orientation.w = qtr.getW();
pose.pose.orientation.x = qtr.getX();
pose.pose.orientation.y = qtr.getY();
pose.pose.orientation.z = qtr.getZ();

// 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 land = false;
int i = 0;
int starting_point = 0;
float angles[] = {180.0, 360.0};

// Hovers inplace for 'counter' iterations
auto loiter_fn = [&](const int& wait_counter)
{
pose.pose.orientation.x = 1;
for (size_t i = 0; i < wait_counter; i++)
{
local_pos_pub.publish(pose);
}
};

// Changes the yaw
auto change_yaw_angle = [&](const float &angle_degree, const int& wait_counter=5)
{
loiter_fn(wait_counter);
pose.pose.orientation.w = qtr.getW();
pose.pose.orientation.x = qtr.getX();
pose.pose.orientation ...
edit retag close merge delete