Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 00:31:36 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

FlightTaskAuto: Nudging: only set yawrate_sp if WV is disabled or stick out of dead-zone

Browse Source 
Otherwise the setpoint from weather vane is constantly overwritten by it,
even if the yaw stick is not moved.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-02-22 10:10:45 +01:00
committed by Matthias Grob
parent 71850eeda6
commit f4c300af25
1 changed files with 8 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+8 -2
View File
@@ -249,8 +249,14 @@ void FlightTaskAuto::_prepareLandSetpoints()
// Stick full up -1 -> stop, stick full down 1 -> double the speed
vertical_speed *= (1 + _sticks.getPositionExpo()(2));
_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading,
_sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime);
// Only set a yawrate setpoint if weather vane is not active or the yaw stick is out of its dead-zone
const bool weather_vane_active = (_ext_yaw_handler != nullptr) && _ext_yaw_handler->is_active();
if (!weather_vane_active || fabsf(_sticks.getPositionExpo()(3)) > FLT_EPSILON) {
_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading,
_sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime);
}
_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _land_heading, _position,
_velocity_setpoint_feedback.xy(), _deltatime);
_stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);