Often times the fastest way to reach the desired gravity vector (what is used in angle mode) is to yaw. This adds yaw to only angle mode and not NFE racer or horizon modes. This also fixes an issue where when a drone is at 90 deg pitch and angle setpoint is at 90 deg roll the angle PID will not ask for any corrections as roll and pitch error will be 0.
Often times the fastest way to reach the desired gravity vector (what is used in angle mode) is to yaw. This adds yaw to only angle mode and not NFE racer or horizon modes. This also fixes an issue where when a drone is at 90 deg pitch and angle setpoint is at 90 deg roll the angle PID will not ask for any corrections as roll and pitch error will be 0.