fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskAuto: Lock down yaw when waypoint is reached

Browse Source 
This is done to prevent yawing 180 degrees when switching
to hold mode with high velocity and the overshoot causing a 180°
yaw turn.

The yaw is unlocked on waypoint updates and the yaw
setpoint generation had to be put at the end because otherwise the
first yaw calculation is with the old waypoint and immediately
locks again.
This commit is contained in:
Matthias Grob 2020-10-15 10:51:12 +02:00
parent 81765bc06a
commit 1fb4f960cd
1 changed files with 26 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp
View File

@ -256,6 +256,26 @@ bool FlightTaskAuto::_evaluateTriplets()
_ext_yaw_handler->deactivate();
}
// Calculate the current vehicle state and check if it has updated.
State previous_state = _current_state;
_current_state = _getCurrentState();
if (triplet_update || (_current_state != previous_state) || _current_state == State::offtrack) {
_updateInternalWaypoints();
_mission_gear = _sub_triplet_setpoint.get().current.landing_gear;
_yaw_lock = false;
}
if (_param_com_obs_avoid.get()
&& _sub_vehicle_status.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
_obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint,
_triplet_next_wp,
_sub_triplet_setpoint.get().next.yaw,
_sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : NAN,
_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint.get().current.type);
_obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt);
}
// set heading
if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) {
_yaw_setpoint = _yaw;
@ -293,25 +313,6 @@ bool FlightTaskAuto::_evaluateTriplets()
_yawspeed_setpoint = NAN;
}
// Calculate the current vehicle state and check if it has updated.
State previous_state = _current_state;
_current_state = _getCurrentState();
if (triplet_update || (_current_state != previous_state) || _current_state == State::offtrack) {
_updateInternalWaypoints();
_mission_gear = _sub_triplet_setpoint.get().current.landing_gear;
}
if (_param_com_obs_avoid.get()
&& _sub_vehicle_status.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
_obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint,
_triplet_next_wp,
_sub_triplet_setpoint.get().next.yaw,
_sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : NAN,
_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint.get().current.type);
_obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt);
}
return true;
}
@ -323,7 +324,7 @@ void FlightTaskAuto::_set_heading_from_mode()
switch (_param_mpc_yaw_mode.get()) {
case 0: // Heading points towards the current waypoint.
case 4: // Same as 0 but yaw fisrt and then go
case 4: // Same as 0 but yaw first and then go
v = Vector2f(_target) - Vector2f(_position);
break;
@ -352,14 +353,13 @@ void FlightTaskAuto::_set_heading_from_mode()
// 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) {
if (!_yaw_lock) {
if (v.length() < _target_acceptance_radius) {
_yaw_setpoint = _yaw;
_yaw_lock = true;
} else {
_compute_heading_from_2D_vector(_yaw_setpoint, v);
}
}