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

Refactor: Use new matrix::Vector2f constructor

Browse Source
This commit is contained in:
Matthias Grob
2018-10-01 11:39:43 +02:00
committed by Daniel Agar
parent ada0179cda
commit bee6a6b8b0
13 changed files with 98 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl.cpp
+4 -4
View File
@@ -219,8 +219,8 @@ void PositionControl::_positionController()
// Constrain horizontal velocity by prioritizing the velocity component along the
// the desired position setpoint over the feed-forward term.
const Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(vel_sp_position(0), vel_sp_position(1)),
Vector2f(&(_vel_sp - vel_sp_position)(0)), _constraints.speed_xy);
const Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(vel_sp_position),
Vector2f(_vel_sp - vel_sp_position), _constraints.speed_xy);
_vel_sp(0) = vel_sp_xy(0);
_vel_sp(1) = vel_sp_xy(1);
// Constrain velocity in z-direction.
@@ -307,7 +307,7 @@ void PositionControl::_velocityController(const float &dt)
}
// Get the direction of (r-y) in NE-direction.
float direction_NE = Vector2f(vel_err(0), vel_err(1)) * Vector2f(_vel_sp(0), _vel_sp(1));
float direction_NE = Vector2f(vel_err) * Vector2f(_vel_sp);
// Apply Anti-Windup in NE-direction.
bool stop_integral_NE = (thrust_desired_NE * thrust_desired_NE >= thrust_max_NE * thrust_max_NE &&
@@ -318,7 +318,7 @@ void PositionControl::_velocityController(const float &dt)
_thr_int(1) += vel_err(1) * MPC_XY_VEL_I.get() * dt;
// magnitude of thrust integral can never exceed maximum throttle in NE
float integral_mag_NE = Vector2f(&_thr_int(0)).length();
float integral_mag_NE = Vector2f(_thr_int).length();
if (integral_mag_NE > 0.0f && integral_mag_NE > thrust_max_NE) {
_thr_int(0) = _thr_int(0) / integral_mag_NE * thrust_max_NE;