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

ControlMath: reference to by value

Browse Source
This commit is contained in:
Dennis Mannhart
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent 16d6ac6ad1
commit fc62df856d
2 changed files with 8 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/Utility/ControlMath.cpp
+6 -5
View File
@@ -58,17 +58,19 @@ namespace ControlMath
* Tilt is adjusted such that vector component in D-direction * Tilt is adjusted such that vector component in D-direction
* has higher priority. * has higher priority.
*/ */
matrix::Vector3f constrainTilt(const matrix::Vector3f &vec, const float &tilt_max) matrix::Vector3f constrainTilt(const matrix::Vector3f &vec, const float maximum_tilt)
{ {
/* We only consider maximum tilt < 90 */ /* We only consider maximum tilt < 90 */
float tilt_max = maximum_tilt;
if (tilt_max > M_PI_2_F) { if (tilt_max > M_PI_2_F) {
return vec; tilt_max = M_PI_2_F;
} }
/* Desired tilt is above 90 -> in order to stay within tilt, /* Desired tilt is above 90 -> in order to stay within tilt,
* vector has to be zero (N-E-D frame)*/ * vector has to be zero (N-E-D frame)*/
if (vec(2) > 0.0f) { if (vec(2) > 0.0f) {
return matrix::Vector3f{}; return matrix::Vector3f();
} }
/* Maximum tilt is 0 */ /* Maximum tilt is 0 */
@@ -160,7 +162,7 @@ void constrainPIDu(matrix::Vector3f &u, bool stop_I[2], const float Ulimits[2],
} }
} }
vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, const float &yaw_sp) vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp)
{ {
vehicle_attitude_setpoint_s att_sp; vehicle_attitude_setpoint_s att_sp;
@@ -225,6 +227,5 @@ vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, con
att_sp.thrust = thr_sp.length(); att_sp.thrust = thr_sp.length();
return att_sp; return att_sp;
} }
} }
src/modules/mc_pos_control/Utility/ControlMath.hpp
+2 -2
View File
@@ -46,7 +46,7 @@
namespace ControlMath namespace ControlMath
{ {
matrix::Vector3f constrainTilt(const matrix::Vector3f &vec, const float &tilt_max); matrix::Vector3f constrainTilt(const matrix::Vector3f &vec, const float tilt_max);
void constrainPIDu(matrix::Vector3f &u, bool stop_I[2], const float Ulimits[2], const float d[2]); void constrainPIDu(matrix::Vector3f &u, bool stop_I[2], const float Ulimits[2], const float d[2]);
vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, const float &yaw_sp); vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp);
} }