fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-05 16:30:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

PositionControl: check and set _skip_controller flag at the end of the setpoint update

Browse Source
This commit is contained in:
Dennis Mannhart
2018-09-26 11:47:45 +02:00
committed by Daniel Agar
parent 2f833a26d1
commit 1f0d559d65
1 changed files with 4 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl.cpp
+4 -9
View File
@@ -56,10 +56,6 @@ void PositionControl::updateState(const PositionControlStates &states)
bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &setpoint)
{
// If full manual is required (thrust already generated), don't run position/velocity
// controller and just return thrust.
_skip_controller = false;
_pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z);
_vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz);
_acc_sp = Vector3f(setpoint.acc_x, setpoint.acc_y, setpoint.acc_z);
@@ -68,9 +64,10 @@ bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &se
_yawspeed_sp = setpoint.yawspeed;
bool mapping_succeeded = _interfaceMapping();
if (PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1]) && PX4_ISFINITE(setpoint.thrust[2])) {
_skip_controller = true;
}
// If full manual is required (thrust already generated), don't run position/velocity
// controller and just return thrust.
_skip_controller = PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1])
&& PX4_ISFINITE(setpoint.thrust[2]);
return mapping_succeeded;
}
@@ -203,8 +200,6 @@ bool PositionControl::_interfaceMapping()
// check failsafe
if (failsafe) {
_skip_controller = true;
// point the thrust upwards
_thr_sp(0) = _thr_sp(1) = 0.0f;
// throttle down such that vehicle goes down with