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

VTOL: Manual Pos Flight Task: do not use yawrate sp from weather vane if position isn't locked 

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

11
src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp
View File

@ -166,12 +166,19 @@ void FlightTaskManualPosition::_updateSetpoints()
{
FlightTaskManualAltitude::_updateSetpoints(); // needed to get yaw and setpoints in z-direction
_updateXYlock(); // check for position lock
// check if an external yaw handler is active and if yes, let it update the yaw setpoints
if (_weathervane_yaw_handler != nullptr && _weathervane_yaw_handler->is_active()) {
_yaw_setpoint = NAN;
_yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate();
// only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. are NAN)
if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) {
// vehicle is steady
_yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate();
}
}
_thrust_setpoint.setAll(NAN); // don't require any thrust setpoints
_updateXYlock(); // check for position lock
}