|
|
|
@ -132,7 +132,7 @@ vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, con
@@ -132,7 +132,7 @@ vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, con
|
|
|
|
|
* constraint: ||vf|| <= max |
|
|
|
|
* solve for s: ||vf|| = ||v0 + s * u1|| <= max |
|
|
|
|
*/ |
|
|
|
|
matrix::Vector2f constrainXY(const matrix::Vector2f &v0, const matrix::Vector2f v1, const float max) |
|
|
|
|
matrix::Vector2f constrainXY(const matrix::Vector2f &v0, const matrix::Vector2f &v1, const float max) |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
if (matrix::Vector2f(v0 + v1).norm() <= max) { |
|
|
|
|