yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskManualPosition: use new Matrix available functions to cast

Browse Source 
from a vector of 4 elements to Vector2f and from Vector2f to Vector3f.
This commit is contained in:
bresch
2019-12-11 15:00:35 +02:00
committed by Mathieu Bresciani
parent d0bdd80129
commit d6ffdc09b8
1 changed files with 6 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp
View File

@@ -84,20 +84,19 @@ void FlightTaskManualPosition::_scaleSticks()
FlightTaskManualAltitude::_scaleSticks(); FlightTaskManualAltitude::_scaleSticks();
/* Constrain length of stick inputs to 1 for xy*/ /* 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) { if (mag > FLT_EPSILON) {
stick_xy = stick_xy.normalized() * mag; stick_xy = stick_xy.normalized() * mag;
} }
// scale the stick inputs const float max_speed_from_estimator = _sub_vehicle_local_position.get().vxy_max;
if (PX4_ISFINITE(_sub_vehicle_local_position.get().vxy_max)) {
// estimator provides vehicle specific max
if (PX4_ISFINITE(max_speed_from_estimator)) {
// use the minimum of the estimator and user specified limit // 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 // Allow for a minimum of 0.3 m/s for repositioning
_velocity_scale = fmaxf(_velocity_scale, 0.3f); _velocity_scale = fmaxf(_velocity_scale, 0.3f);
@@ -119,8 +118,7 @@ void FlightTaskManualPosition::_scaleSticks()
Vector2f(_velocity)); Vector2f(_velocity));
} }
_velocity_setpoint(0) = vel_sp_xy(0); _velocity_setpoint.xy() = vel_sp_xy;
_velocity_setpoint(1) = vel_sp_xy(1);
} }
float FlightTaskManualPosition::_computeVelXYGroundDist() float FlightTaskManualPosition::_computeVelXYGroundDist()