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

mc_pos_control: check if position-control setpoint update succeeded.

Browse Source 
refactor use_obstacle_avoidance method
This commit is contained in:
Dennis Mannhart
2018-09-12 10:18:41 +02:00
committed by Daniel Agar
parent b83b588fc5
commit 2f833a26d1
3 changed files with 24 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl.cpp
+7 -3
View File
@@ -54,7 +54,7 @@ void PositionControl::updateState(const PositionControlStates &states)
_vel_dot = states.acceleration;
}
void PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &setpoint)
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.
@@ -66,11 +66,13 @@ void PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &se
_thr_sp = Vector3f(setpoint.thrust);
_yaw_sp = setpoint.yaw;
_yawspeed_sp = setpoint.yawspeed;
_interfaceMapping();
bool mapping_succeeded = _interfaceMapping();
if (PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1]) && PX4_ISFINITE(setpoint.thrust[2])) {
_skip_controller = true;
}
return mapping_succeeded;
}
void PositionControl::generateThrustYawSetpoint(const float dt)
@@ -98,7 +100,7 @@ void PositionControl::generateThrustYawSetpoint(const float dt)
}
}
void PositionControl::_interfaceMapping()
bool PositionControl::_interfaceMapping()
{
// if noting is valid, then apply failsafe landing
bool failsafe = false;
@@ -209,6 +211,8 @@ void PositionControl::_interfaceMapping()
// 70% of throttle range between min and hover
_thr_sp(2) = -(MPC_THR_MIN.get() + (MPC_THR_HOVER.get() - MPC_THR_MIN.get()) * 0.7f);
}
return !(failsafe);
}
void PositionControl::_positionController()