// set heading used for spline and waypoint navigation
void AC_WPNav::set_yaw_cd(float heading_cd)
{

    _yaw = heading_cd;
    _flags.wp_yaw_set = true;
            if(_yaw < 0.01f){
        _flags.wp_yaw_set = false;
        }
}


// mark byte
    // update the target yaw if origin and destination are at least 2m apart horizontally
    // if (_track_length_xy >= WPNAV_YAW_DIST_MIN) {
    //     if (_pos_control.get_leash_xy() < WPNAV_YAW_DIST_MIN) {
        
            
            // if the leash is short (i.e. moving slowly) and destination is at least 2m horizontally, point along the segment from origin to destination
           
            
    //     } else {
             Vector3f horiz_leash_xy = final_target - curr_pos;
             horiz_leash_xy.z = 0;
             if (horiz_leash_xy.length() > MIN(10, _pos_control.get_leash_xy()*WPNAV_YAW_LEASH_PCT_MIN)) {
               set_yaw_cd(get_bearing_cd(_origin, _destination));
               gcs().send_text(MAV_SEVERITY_INFO, "get_bearing_cd= %f",get_bearing_cd(_origin, _destination));
                // set_yaw_cd(RadiansToCentiDegrees(atan2f(horiz_leash_xy.y,horiz_leash_xy.x)));
             }else{
                _flags.wp_yaw_set = false;
    //              _yaw = _ahrs.yaw*100;
    //                  if (_yaw < 0) {
    //     _yaw += 36000.0f;
    // }
                  gcs().send_text(MAV_SEVERITY_INFO, "_ahrs.yaw*100= %f",_yaw);
             }
    //     }
    // }

    // successfully advanced along track
    return true;
}


#include <GCS_MAVLink/GCS_MAVLink.h>
#include <GCS_MAVLink/GCS.h>
// yaw - returns target heading depending upon auto_yaw.mode()
float Mode::AutoYaw::yaw()
{
    switch (_mode) {

    case AUTO_YAW_ROI:
        // point towards a location held in roi
        return roi_yaw();

    case AUTO_YAW_FIXED:
        // keep heading pointing in the direction held in fixed_yaw
        // with no pilot input allowed
        return _fixed_yaw;

    case AUTO_YAW_LOOK_AHEAD:
        // Commanded Yaw to automatically look ahead.
        return look_ahead_yaw();

    case AUTO_YAW_RESETTOARMEDYAW:
        // changes yaw to be same as when quad was armed
        return copter.initial_armed_bearing;

    case AUTO_YAW_LOOK_AT_NEXT_WP:
         return copter.wp_nav->get_yaw();
        // return copter.wp_nav->get_wp_bearing_to_destination();
    default:
        // point towards next waypoint.
        // we don't use wp_bearing because we don't want the copter to turn too much during flight
        return copter.wp_nav->get_yaw();
    }
}


void ModeGuided::pos_control_run()
{
    // process pilot's yaw input
    float target_yaw_rate = 0;
    if (!copter.failsafe.radio) {
        // get pilot's desired yaw rate
        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
        if (!is_zero(target_yaw_rate)) {
            auto_yaw.set_mode(AUTO_YAW_HOLD);
        }
    }

    // if not armed set throttle to zero and exit immediately
    if (is_disarmed_or_landed()) {
        make_safe_spool_down();
        return;
    }

    // set motors to full range
    motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

    // run waypoint controller
    copter.failsafe_terrain_set_status(wp_nav->update_wpnav());

    // call z-axis position controller (wpnav should have already updated it's alt target)
    pos_control->update_z_controller();
    auto_yaw.set_mode(AUTO_YAW_LOOK_AT_NEXT_WP);
    // call attitude controller
    // if (auto_yaw.mode() == AUTO_YAW_HOLD) {
    //     // roll & pitch from waypoint controller, yaw rate from pilot
    //     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
    // } else if (auto_yaw.mode() == AUTO_YAW_RATE) {
    //     // roll & pitch from waypoint controller, yaw rate from mavlink command or mission item
    //     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.rate_cds());
    // } else {
        // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
        // gcs().send_text(MAV_SEVERITY_INFO, "SEND TEXT =%f ",auto_yaw.yaw());
        attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true);
    // }
}

``