From 3d591f4f8c7a48f6aa35e38974b0f4689db3416b Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis.mannhart@gmail.com>
Date: Tue, 9 Jan 2018 08:58:35 +0100
Subject: [PATCH] FlightTaskManualAltitude: thrust along xy depends on thrust
along z direction
---
.../tasks/FlightTaskManualAltitude.cpp | 17 ++++++++++++-
.../mc_pos_control/PositionControl.cpp | 24 +++++++++++++++----
2 files changed, 35 insertions(+), 6 deletions(-)
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
index 3d46a92090..eadd74d83d 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
@@ -88,7 +88,22 @@ void FlightTaskManualAltitude::_updateZsetpoints()
void FlightTaskManualAltitude::_updateSetpoints()
{
- FlightTaskManualStabilized::_updateSetpoints(); // get yaw and thrust setpoints
+ FlightTaskManualStabilized::_updateSetpoints(); // get yaw setpoint
+ /* Thrust in xy are extracted directly from stick inputs. A magnitude of
+ * 1 means that maximum thrust along xy is required. A magnitude of 0 means no
+ * thrust along xy is required. The maximum thrust along xy depends on the thrust
+ * setpoint along z-direction, which is computed in PositionControl.cpp.
+ */
+ matrix::Vector3f sp{_sticks(0), _sticks(1), 0.0f};
+ sp = (matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, _yaw)) * sp);
+
+ if (sp.length() > 1.0f) {
+ sp.normalize();
+ }
+
+ _thr_sp(0) = sp(0);
+ _thr_sp(1) = sp(1);
+
_updateZsetpoints(); // get z setpoints
}
diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp
index c040662c6f..a3abe51f03 100644
--- a/src/modules/mc_pos_control/PositionControl.cpp
+++ b/src/modules/mc_pos_control/PositionControl.cpp
@@ -225,14 +225,28 @@ void PositionControl::_velocityController(const float &dt)
/* TODO: add offboard acceleration mode
* PID-controller */
Data offset(0.0f, 0.0f, _ThrHover);
- _thr_sp = Pv.emult(vel_err) + Dv.emult(_vel_dot) + _thr_int - offset + _thr_sp;
+ Data thr_sp = Pv.emult(vel_err) + Dv.emult(_vel_dot) + _thr_int - offset;
- /* Limit tilt with priority on z
- * For manual controlled mode excluding pure manual and rate control, maximum tilt is 90;
- * It is to note that pure manual and rate control will never enter _velocityController method.
- * TODO: This needs to be revisited. */
+ /* Get maximum tilt */
float tilt_max = PX4_ISFINITE(_constraints.tilt_max) ? _constraints.tilt_max : M_PI_2_F;
tilt_max = math::min(tilt_max, M_PI_2_F);
+
+ /* Compute maximum allowed thrust along xy based on thrust in z-direction and
+ * scale _thrust_sp by that value. This only has an effect for altitude mode where
+ * _thr_sp(0:1).length() > 0.0f.
+ */
+ if (matrix::Vector2f(&_thr_sp(0)).length() > FLT_EPSILON) {
+
+ float thr_xy_max = fabsf(thr_sp(2)) * tanf(tilt_max);
+ _thr_sp(0) *= thr_xy_max;
+ _thr_sp(1) *= thr_xy_max;
+
+ }
+
+ _thr_sp += thr_sp;
+ /* Limit tilt with priority on z
+ * For manual controlled mode excluding pure manual and rate control, maximum tilt is 90;
+ * It is to note that pure manual and rate control will never enter _velocityController method. */
_thr_sp = ControlMath::constrainTilt(_thr_sp, tilt_max);
--
GitLab