# Controller

**URL:** <https://discourse.openrobotics.org/t/controller/4060>\
**Category:** Projects\
**Tags:** kinetic\
**Created:** [February 27, 2018, 7:14pm UTC](https://discourse.openrobotics.org/t/controller/4060 "2018-02-27T19:14:55Z")\
**Posts on this page:** 1\
**Showing post:** 1

<div class="post-metadata">

**Author:** ![cfis](https://avatars.discourse-cdn.com/v4/letter/c/dc4da7/32.png) [@cfis](https://discourse.openrobotics.org/u/cfis)\
**Post date:** [February 27, 2018, 7:14pm UTC](https://discourse.openrobotics.org/t/controller/4060/1 "2018-02-27T19:14:55Z")

</div>

Good evening dear community of ROS, I am a Cuban boy of 12 years who by chance discovered the simulator gazebo ros, but due to the poor communication in my country I have limited access to the Internet. Currently I set my goal to develop the simulation of a quadrator in ros, which is able to locate a mobile platform, and land on it, but I have stumbled on the first problem to achieve it, I do not know how to implement a controller that allows me to rotate the quadrator on the z-axis, that is, its yaw, from its rotation matrix.

Best regards, God bless you.

PS: I attached a part of the code, in which you can see the way I currently perform the rotation without using the rotation matrix.

//-------------- YAW CONTROL ----------------  
max\_grad = 180.0;  
yaw\_error = yaw\_reference - yaw;  
u = kp\_yaw\*yaw\_error/max\_grad;

```
		commandPilot.angular.z = u;
		

		//------------ ALTITUDE CONTROL ---.---------

		altura_error = alt_ref - altitude;
		commandPilot.linear.z = kp_alt * altura_error;
			

		if(commandPilot.linear.z > 0.5)
			commandPilot.linear.z = 0.5;
	
		if(fabsf(altura_error) < 0.01 && fabsf(yaw_error) < 0.5)
		{
			state = 1;
			commandPilot.linear.x = 0;
			commandPilot.linear.y = 0;
			pose_xi = start_pose.position.x;
			pose_yi = start_pose.position.y;
			//~ ROS_INFO_STREAM("Gazebo pose: X = " << pose_xi << " Y = " << pose_yi);
		}				
		
	}else
	if(state == 1)
	{	
		putText(front_image," Dist: "+ stDist.str() + " Dist error: "+ stErrorDist.str() + " Signal: " + stSignal.str(), Point( 5, 55), FONT_HERSHEY_COMPLEX_SMALL, 0.7, CV_RGB( 255, 255, 255), 1, 1);
	
		dist_x = distancia - fabsf(pose_xi);
		dist_error = dist_x - start_pose.position.x;
		signal = kp * dist_error;
		
		commandPilot.linear.x = signal;
		
		if(fabsf(dist_x) < 0.01)
		{
			commandPilot.linear.x = 0;
			commandPilot.linear.y = 0;				
		}			
	}

```

void image\_viewer::Ardrone\_odom(const sensor\_msgs::Imu::ConstPtr &odom)  
{   
odom\_ARdrone\_x = odom-\>orientation.x;  
odom\_ARdrone\_y = odom-\>orientation.y;  
odom\_ARdrone\_z = odom-\>orientation.z;  
odom\_ARdrone\_w = odom-\>orientation.w;

```
//my code

/*x0 = odom_ARdrone_x;
y0 = odom_ARdrone_y;
z0 = odom_ARdrone_z;
w0 = odom_ARdrone_w;*/

tf::Quaternion q1(odom_ARdrone_x,odom_ARdrone_y,odom_ARdrone_z,odom_ARdrone_w);
tf::Matrix3x3 m(q1);

//Found final position

//z1 = r31x0 + r32y0 + r33z0

m.getRPY(roll, pitch, yaw);

roll = (roll * 180) / M_PI;
pitch = (pitch * 180) / M_PI;
yaw = (yaw * 180) / M_PI;	

```

}

---

_[View the full topic](https://discourse.openrobotics.org/t/controller/4060)._