void FlightTaskAuto::_set_heading_from_mode() { Vector2f v; // Vector that points towards desired location switch (MPC_YAW_MODE.get()) { case 0: // Heading points towards the current waypoint. v = Vector2f(_target) - Vector2f(_position); break; case 1: // Heading points towards home. if (_sub_home_position->get().valid_hpos) { v = Vector2f(&_sub_home_position->get().x) - Vector2f(_position); } break; case 2: // Heading point away from home. if (_sub_home_position->get().valid_hpos) { v = Vector2f(_position) - Vector2f(&_sub_home_position->get().x); } break; case 3: // Along trajectory. // The heading depends on the kind of setpoint generation. This needs to be implemented // in the subclasses where the velocity setpoints are generated. v.setAll(NAN); break; } if (PX4_ISFINITE(v.length())) { // We only adjust yaw if vehicle is outside of acceptance radius. Once we enter acceptance // radius, lock yaw to current yaw. // This prevents excessive yawing. if (v.length() > _target_acceptance_radius) { _compute_heading_from_2D_vector(_yaw_setpoint, v); _yaw_lock = false; } else { if (!_yaw_lock) { _yaw_setpoint = _yaw; _yaw_lock = true; } } } else { _yaw_lock = false; _yaw_setpoint = NAN; } }