How to take 2 sets of coordinates and altitude and calculate angle for Servos

Projects Programming
3

Hello, something like that should work (not sure it will work in your application!):

void GetAngles3D( float x1, float y1, float z1, float x2, float y2, float z2, float &yaw, float &pitch )
{
  x2 -= x1;
  y2 -= y1;
  z2 -= z1;
  yaw = atan2( y2, x2 ) * 57.29578;
  pitch = atan2( z2, sqrt( (x2 * x2) + (y2 * y2) ) ) * 57.29578;
}

Returned angles in degrees, remove " * 57.29578" for radians... Good luck :slight_smile: