function Hold_Yaw  = fcn(Yaw_hold,pop,hold,ret)

persistent WP_hold WP_index_hold

if isempty(WP_hold)
    WP_hold = zeros(1,100000);
    WP_index_hold = 1;
    WP_hold(1,WP_index_hold) = Yaw_hold;
end

if pop
    pop_stack = 1;
else
    pop_stack = 0;
end

if hold && ~ret
    if WP_index_hold > 1
        Hold_Yaw = WP_hold(1,WP_index_hold);
    else
        Hold_Yaw = WP_hold(1,1);
    end
    
    if pop_stack && WP_index_hold > 1
        WP_index_hold = WP_index_hold - 1;
    end
else
    Hold_Yaw = Yaw_hold;
    if norm(WP_hold(1,WP_index_hold)-Yaw_hold) > 0.01
        WP_index_hold = WP_index_hold + 1;
        WP_hold(1,WP_index_hold) = Yaw_hold;
    end
end
end