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;
}
}
