fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-03 02:50:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskManualStabilized: check for 0 maximum tilt and scale throttle linearly

Browse Source
This commit is contained in:
Dennis Mannhart
2018-02-15 12:53:54 +01:00
committed by Beat Küng
parent 7f406aa251
commit 16d6ac6ad1
1 changed files with 9 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp
+9 -5
View File
@@ -85,8 +85,12 @@ void FlightTaskManualStabilized::_updateThrustSetpoints()
/* Rotate setpoint into local frame. */
matrix::Vector3f sp{_sticks(0), _sticks(1), 0.0f};
sp = (matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, _yaw)) * sp);
const float x = sp(0) * math::radians(_tilt_max_man.get());
const float y = sp(1) * math::radians(_tilt_max_man.get());
/* Ensure that maximum tilt is in [0.001, Pi] */
float tilt_max = math::constrain(math::radians(_tilt_max_man.get()), 0.001f, M_PI_F);
const float x = sp(0) * tilt_max;
const float y = sp(1) * tilt_max;
/* The norm of the xy stick input provides the pointing
* direction of the horizontal desired thrust setpoint. The magnitude of the
@@ -97,8 +101,8 @@ void FlightTaskManualStabilized::_updateThrustSetpoints()
matrix::Vector2f v = matrix::Vector2f(y, -x);
float v_norm = v.norm(); // the norm of v defines the tilt angle
if (v_norm > _tilt_max_man.get()) { // limit to the configured maximum tilt angle
v *= _tilt_max_man.get() / v_norm;
if (v_norm > tilt_max) { // limit to the configured maximum tilt angle
v *= tilt_max / v_norm;
}
/* The final thrust setpoint is found by rotating the scaled unit vector pointing
@@ -118,7 +122,7 @@ float FlightTaskManualStabilized::_throttleCurve()
{
/* Scale stick z from [-1,1] to [min thrust, max thrust]
* with hover throttle at 0.5 stick */
float throttle = -((_sticks_expo(2) - 1.0f) * 0.5f);
float throttle = -((_sticks(2) - 1.0f) * 0.5f);
if (throttle < 0.5f) {
return (_throttle_hover.get() - _throttle_min.get()) / 0.5f * throttle + _throttle_min.get();