dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

VTOL: flight task auto: disable weather vane when not stationary 

Use yawrate setpoint from weather vane only if velocity setpoint is small
(smaller than half the cruise velocity)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2019-12-28 18:42:08 +01:00 committed by Beat Küng
parent 98aee29ddc
commit a3710fcdef
1 changed files with 12 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -248,7 +248,18 @@ bool FlightTaskAuto::_evaluateTriplets()
// set heading
if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) {
_yaw_setpoint = _yaw;
_yawspeed_setpoint = _ext_yaw_handler->get_weathervane_yawrate();
// use the yawrate setpoint from WV only if not moving lateral (velocity setpoint below half of _param_mpc_xy_cruise)
// otherwise, keep heading constant (as output from WV is not according to wind in this case)
bool vehicle_is_moving_lateral = _velocity_setpoint.xy().longerThan(_param_mpc_xy_cruise.get() / 2.0f);
if (vehicle_is_moving_lateral) {
_yawspeed_setpoint = 0.0f;
} else {
_yawspeed_setpoint = _ext_yaw_handler->get_weathervane_yawrate();
}
} else if (_type == WaypointType::follow_target && _sub_triplet_setpoint.get().current.yawspeed_valid) {
_yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed;