Control quadcopter header facing to next setpoint within offboard mode

I am really new to robotic field, I want to make my copter navigate to next setpoint by turning it header facing to the upcomming setpoint first before move to that setpoint. As i know so far, I can use yaw and yaw_rate in SET_POSITION_TARGET_LOCAL_NED, and i can get current orientation of the copter via. ATTITUDE, but i don’t know which algorithm I should use. Could you please suggest me about this.

.
Thank you in advance :slight_smile:

Could you be more accurate on what you want please ? Because you have totally explain the way to make what you want to do ^^’

if you mean : current setpoint -> turn the drone in the direction of the next setpoint -> go to the next setpoint

you can do it like that I think :
if ( current_message.attitude.yaw* - current_message.set_position_target_local_ned.yaw** < 0.1 rad (for example))
{
new_setpoint();
}

  • attitude.yaw : -pi to +pi
    ** it depends of your mav_frame