From 26ffda507968b44677721060502800c2ff6ffc72 Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis@yuneecresearch.com>
Date: Fri, 9 Mar 2018 13:02:39 +0100
Subject: [PATCH] PositionControl: replace matrix length() method with simple
dot-product to safe compuation
---
src/modules/mc_pos_control/PositionControl.cpp | 14 ++++++++------
1 file changed, 8 insertions(+), 6 deletions(-)
diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp
index 92f5114cb5..ee492f8c3a 100644
--- a/src/modules/mc_pos_control/PositionControl.cpp
+++ b/src/modules/mc_pos_control/PositionControl.cpp
@@ -246,7 +246,7 @@ void PositionControl::_velocityController(const float &dt)
/* Satureate thrust setpoint in D-direction */
_thr_sp(2) = math::constrain(thrust_desired_D, uMin, uMax);
- if (matrix::Vector2f(&_thr_sp(0)).length() > FLT_EPSILON) {
+ if (_thr_sp(0) + _thr_sp(1) > FLT_EPSILON) {
/* Thrust setpoints in NE-direction is already provided. Only
* scaling is required.
@@ -265,14 +265,15 @@ void PositionControl::_velocityController(const float &dt)
/* Get maximum allowed thrust in NE based on tilt and excess thrust */
float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(tilt_max);
- float thrust_max_NE = sqrtf(_ThrustLimit.max * _ThrustLimit.max - fabsf(_thr_sp(2) * fabsf(_thr_sp(2))));
+ float thrust_max_NE = sqrtf(_ThrustLimit.max * _ThrustLimit.max - _thr_sp(2) * _thr_sp(2));
thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE);
/* Get the direction of (r-y) in NE-direction */
float direction_NE = Vector2f(&vel_err(0)) * Vector2f(&_vel_sp(0));
/* Apply Anti-Windup in NE-direction */
- bool stop_integral_NE = (thrust_desired_NE.length() >= thrust_max_NE && direction_NE >= 0.0f);
+ bool stop_integral_NE = (thrust_desired_NE * thrust_desired_NE >= thrust_max_NE * thrust_max_NE &&
+ direction_NE >= 0.0f);
if (!stop_integral_NE) {
_thr_int(0) += vel_err(0) * Iv(0) * dt;
@@ -283,9 +284,10 @@ void PositionControl::_velocityController(const float &dt)
_thr_sp(0) = thrust_desired_NE(0);
_thr_sp(1) = thrust_desired_NE(1);
- if (thrust_desired_NE.length() > thrust_max_NE) {
- _thr_sp(0) = thrust_desired_NE(0) / thrust_desired_NE.length() * thrust_max_NE;
- _thr_sp(1) = thrust_desired_NE(1) / thrust_desired_NE.length() * thrust_max_NE;
+ if (thrust_desired_NE * thrust_desired_NE > thrust_max_NE * thrust_max_NE) {
+ float mag = thrust_desired_NE.length();
+ _thr_sp(0) = thrust_desired_NE(0) / mag * thrust_max_NE;
+ _thr_sp(1) = thrust_desired_NE(1) / mag * thrust_max_NE;
}
}
--
GitLab