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

move takeoff state machine flight_mode_manager -> mc_pos_control

Browse Source
This commit is contained in:
Daniel Agar
2021-02-16 12:26:14 -05:00
committed by Matthias Grob
parent 823c6078d9
commit 266ea377da
20 changed files with 288 additions and 279 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+2 -23
View File
@@ -88,27 +88,6 @@ void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &
_yawspeed_sp = setpoint.yawspeed;
}
void PositionControl::setConstraints(const vehicle_constraints_s &constraints)
{
_constraints = constraints;
// For safety check if adjustable constraints are below global constraints. If they are not stricter than global
// constraints, then just use global constraints for the limits.
if (!PX4_ISFINITE(constraints.tilt) || (constraints.tilt > _lim_tilt)) {
_constraints.tilt = _lim_tilt;
}
if (!PX4_ISFINITE(constraints.speed_up) || (constraints.speed_up > _lim_vel_up)) {
_constraints.speed_up = _lim_vel_up;
}
if (!PX4_ISFINITE(constraints.speed_down) || (constraints.speed_down > _lim_vel_down)) {
_constraints.speed_down = _lim_vel_down;
}
// ignore _constraints.speed_xy TODO: remove it completely as soon as no task uses it anymore to avoid confusion
}
bool PositionControl::update(const float dt)
{
// x and y input setpoints always have to come in pairs
@@ -138,7 +117,7 @@ void PositionControl::_positionControl()
// the desired position setpoint over the feed-forward term.
_vel_sp.xy() = ControlMath::constrainXY(vel_sp_position.xy(), (_vel_sp - vel_sp_position).xy(), _lim_vel_horizontal);
// Constrain velocity in z-direction.
_vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.speed_up, _constraints.speed_down);
_vel_sp(2) = math::constrain(_vel_sp(2), -_lim_vel_up, _lim_vel_down);
}
void PositionControl::_velocityControl(const float dt)
@@ -198,7 +177,7 @@ void PositionControl::_accelerationControl()
{
// Assume standard acceleration due to gravity in vertical direction for attitude generation
Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized();
ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _constraints.tilt);
ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt);
// Scale thrust assuming hover thrust produces standard gravity
float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;
// Project thrust to planned body attitude