From d6ffdc09b82f4ba9b922e4230a594d6bf50d5bc8 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 11 Dec 2019 15:00:35 +0200 Subject: [PATCH] FlightTaskManualPosition: use new Matrix available functions to cast from a vector of 4 elements to Vector2f and from Vector2f to Vector3f. --- .../ManualPosition/FlightTaskManualPosition.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index ec38e85d2d..57724e5f33 100644 --- a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -84,20 +84,19 @@ void FlightTaskManualPosition::_scaleSticks() FlightTaskManualAltitude::_scaleSticks(); /* Constrain length of stick inputs to 1 for xy*/ - Vector2f stick_xy(&_sticks_expo(0)); + Vector2f stick_xy = _sticks_expo.slice<2, 1>(0, 0); - float mag = math::constrain(stick_xy.length(), 0.0f, 1.0f); + const float mag = math::constrain(stick_xy.length(), 0.0f, 1.0f); if (mag > FLT_EPSILON) { stick_xy = stick_xy.normalized() * mag; } - // scale the stick inputs - if (PX4_ISFINITE(_sub_vehicle_local_position.get().vxy_max)) { - // estimator provides vehicle specific max + const float max_speed_from_estimator = _sub_vehicle_local_position.get().vxy_max; + if (PX4_ISFINITE(max_speed_from_estimator)) { // use the minimum of the estimator and user specified limit - _velocity_scale = fminf(_constraints.speed_xy, _sub_vehicle_local_position.get().vxy_max); + _velocity_scale = fminf(_constraints.speed_xy, max_speed_from_estimator); // Allow for a minimum of 0.3 m/s for repositioning _velocity_scale = fmaxf(_velocity_scale, 0.3f); @@ -119,8 +118,7 @@ void FlightTaskManualPosition::_scaleSticks() Vector2f(_velocity)); } - _velocity_setpoint(0) = vel_sp_xy(0); - _velocity_setpoint(1) = vel_sp_xy(1); + _velocity_setpoint.xy() = vel_sp_xy; } float FlightTaskManualPosition::_computeVelXYGroundDist()