From eaf4f99e38bb0ee6a46e4be3c196a3483aa1a7cc Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Wed, 28 Feb 2018 10:02:20 +0100
Subject: [PATCH] FlightTasks: adapt tasks to member setpoints
The FlightTaskManual... tasks already had their internal
setpoint member variables. I switched them to use the
architecture with setpoint member variables as it was
implemented the commit before. They simplify a lot.
---
.../FlightTasks/tasks/FlightTaskManual.cpp | 9 -------
.../FlightTasks/tasks/FlightTaskManual.hpp | 9 -------
.../tasks/FlightTaskManualAltitude.cpp | 23 ++++++-----------
.../tasks/FlightTaskManualAltitude.hpp | 2 --
.../tasks/FlightTaskManualAltitudeSmooth.cpp | 2 +-
.../tasks/FlightTaskManualPosition.cpp | 25 +++++++------------
.../tasks/FlightTaskManualPosition.hpp | 2 --
.../tasks/FlightTaskManualPositionSmooth.cpp | 10 ++++----
.../tasks/FlightTaskManualStabilized.cpp | 25 +++++++------------
src/lib/FlightTasks/tasks/FlightTaskSport.hpp | 8 +++---
10 files changed, 36 insertions(+), 79 deletions(-)
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp
index 827c2c9641..813a3b8b29 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp
@@ -98,12 +98,3 @@ bool FlightTaskManual::_evaluateSticks()
return false;
}
}
-
-void FlightTaskManual::_resetToNAN()
-{
- _thr_sp *= NAN;
- _vel_sp *= NAN;
- _pos_sp *= NAN;
- _yaw_sp = NAN;
- _yaw_rate_sp = NAN;
-}
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp
index 33dce897b7..fc3307a55f 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp
@@ -63,15 +63,6 @@ protected:
matrix::Vector3f _sticks_expo; /**< modified manual sticks using expo function*/
control::BlockParamFloat _stick_dz; /**< 0-deadzone around the center for the sticks */
- /* Setpoints: NAN means that setpoint is not being considered. */
- matrix::Vector3f _thr_sp{NAN, NAN, NAN}; /**< thrust setpoint */
- matrix::Vector3f _vel_sp{NAN, NAN, NAN}; /**< velocity setpoint */
- matrix::Vector3f _pos_sp{NAN, NAN, NAN}; /**< position setpoint */
- float _yaw_sp{NAN}; /**< yaw setpoint */
- float _yaw_rate_sp{NAN}; /**< yawspeed setpoint */
-
- void _resetToNAN();
-
private:
uORB::Subscription<manual_control_setpoint_s> *_sub_manual_control_setpoint{nullptr};
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
index fa231d9476..1a75545067 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
@@ -48,13 +48,6 @@ FlightTaskManualAltitude::FlightTaskManualAltitude(control::SuperBlock *parent,
{}
-bool FlightTaskManualAltitude::activate()
-{
- bool ret = FlightTaskManualStabilized::activate();
- _vel_sp(2) = 0.0f;
- return ret;
-}
-
void FlightTaskManualAltitude::_scaleSticks()
{
/* Reuse same scaling as for stabilized */
@@ -62,7 +55,7 @@ void FlightTaskManualAltitude::_scaleSticks()
/* Scale horizontal velocity with expo curve stick input*/
const float vel_max_z = (_sticks(2) > 0.0f) ? _vel_max_down.get() : _vel_max_up.get();
- _vel_sp(2) = vel_max_z * _sticks_expo(2);
+ _velocity_setpoint(2) = vel_max_z * _sticks_expo(2);
}
void FlightTaskManualAltitude::_updateAltitudeLock()
@@ -72,14 +65,14 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
*/
/* handle position and altitude hold */
- const bool apply_brake_z = fabsf(_vel_sp(2)) <= FLT_EPSILON;
+ const bool apply_brake_z = fabsf(_velocity_setpoint(2)) <= FLT_EPSILON;
const bool stopped_z = (_vel_hold_thr_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _vel_hold_thr_z.get());
- if (apply_brake_z && stopped_z && !PX4_ISFINITE(_pos_sp(2))) {
- _pos_sp(2) = _position(2);
+ if (apply_brake_z && stopped_z && !PX4_ISFINITE(_position_setpoint(2))) {
+ _position_setpoint(2) = _position(2);
} else if (!apply_brake_z) {
- _pos_sp(2) = NAN;
+ _position_setpoint(2) = NAN;
}
}
@@ -87,7 +80,7 @@ void FlightTaskManualAltitude::_updateSetpoints()
{
FlightTaskManualStabilized::_updateSetpoints(); // get yaw and thrust setpoints
- _thr_sp *= NAN; // Don't need thrust setpoint from Stabilized mode.
+ _thrust_setpoint *= NAN; // Don't need thrust setpoint from Stabilized mode.
/* 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
@@ -101,8 +94,8 @@ void FlightTaskManualAltitude::_updateSetpoints()
sp.normalize();
}
- _thr_sp(0) = sp(0);
- _thr_sp(1) = sp(1);
+ _thrust_setpoint(0) = sp(0);
+ _thrust_setpoint(1) = sp(1);
_updateAltitudeLock();
}
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
index b6b5d0d385..293003c3e4 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
@@ -48,8 +48,6 @@ public:
virtual ~FlightTaskManualAltitude() = default;
- bool activate() override;
-
protected:
control::BlockParamFloat _vel_max_down; /**< maximum speed allowed to go up */
control::BlockParamFloat _vel_max_up; /**< maximum speed allowed to go down */
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitudeSmooth.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitudeSmooth.cpp
index f319e8e830..bb0c5485de 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitudeSmooth.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitudeSmooth.cpp
@@ -50,7 +50,7 @@ void FlightTaskManualAltitudeSmooth::_updateSetpoints()
FlightTaskManualAltitude::_updateSetpoints();
/* Smooth velocity in z*/
- _smoothing.smoothVelFromSticks(_vel_sp(2), _deltatime);
+ _smoothing.smoothVelFromSticks(_velocity_setpoint(2), _deltatime);
/* Check for altitude lock*/
_updateAltitudeLock();
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
index 2067b5c04c..d84050c8d5 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
@@ -48,13 +48,6 @@ FlightTaskManualPosition::FlightTaskManualPosition(control::SuperBlock *parent,
_vel_hold_thr_xy(parent, "MPC_HOLD_MAX_XY", false)
{}
-bool FlightTaskManualPosition::activate()
-{
- bool ret = FlightTaskManualAltitude::activate();
- _vel_sp(0) = _vel_sp(1) = 0.0f;
- return ret;
-}
-
void FlightTaskManualPosition::_scaleSticks()
{
/* Use same scaling as for FlightTaskManualAltitude */
@@ -74,31 +67,31 @@ void FlightTaskManualPosition::_scaleSticks()
/* Rotate setpoint into local frame. */
_rotateIntoHeadingFrame(vel_sp_xy);
- _vel_sp(0) = vel_sp_xy(0);
- _vel_sp(1) = vel_sp_xy(1);
+ _velocity_setpoint(0) = vel_sp_xy(0);
+ _velocity_setpoint(1) = vel_sp_xy(1);
}
void FlightTaskManualPosition::_updateXYlock()
{
/* If position lock is not active, position setpoint is set to NAN.*/
const float vel_xy_norm = Vector2f(&_velocity(0)).length();
- const bool apply_brake = Vector2f(&_vel_sp(0)).length() < FLT_EPSILON;
+ const bool apply_brake = Vector2f(&_velocity_setpoint(0)).length() < FLT_EPSILON;
const bool stopped = (_vel_hold_thr_xy.get() < FLT_EPSILON || vel_xy_norm < _vel_hold_thr_xy.get());
- if (apply_brake && stopped && !PX4_ISFINITE(_pos_sp(0))) {
- _pos_sp(0) = _position(0);
- _pos_sp(1) = _position(1);
+ if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(0))) {
+ _position_setpoint(0) = _position(0);
+ _position_setpoint(1) = _position(1);
} else if (!apply_brake) {
/* don't lock*/
- _pos_sp(0) = NAN;
- _pos_sp(1) = NAN;
+ _position_setpoint(0) = NAN;
+ _position_setpoint(1) = NAN;
}
}
void FlightTaskManualPosition::_updateSetpoints()
{
FlightTaskManualAltitude::_updateSetpoints(); // needed to get yaw and setpoints in z-direction
- _thr_sp *= NAN; // don't require any thrust setpoints
+ _thrust_setpoint *= NAN; // don't require any thrust setpoints
_updateXYlock(); // check for position lock
}
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp
index e9a9929e00..2277a48e77 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp
@@ -49,8 +49,6 @@ public:
virtual ~FlightTaskManualPosition() = default;
- bool activate() override;
-
protected:
control::BlockParamFloat _vel_xy_manual_max; /**< maximum speed allowed horizontally */
control::BlockParamFloat _acc_xy_max;/**< maximum acceleration horizontally. Only used to compute lock time */
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp
index 172c0c090b..a8507911f5 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp
@@ -52,16 +52,16 @@ void FlightTaskManualPositionSmooth::_updateSetpoints()
/* Smooth velocity setpoint in xy.*/
matrix::Vector2f vel(&_velocity(0));
- Vector2f vel_sp_xy = Vector2f(&_vel_sp(0));
- _smoothingXY.smoothVelocity(vel_sp_xy, vel, _yaw, _yaw_rate_sp, _deltatime);
- _vel_sp(0) = vel_sp_xy(0);
- _vel_sp(1) = vel_sp_xy(1);
+ Vector2f vel_sp_xy = Vector2f(&_velocity_setpoint(0));
+ _smoothingXY.smoothVelocity(vel_sp_xy, vel, _yaw, _yawspeed_setpoint, _deltatime);
+ _velocity_setpoint(0) = vel_sp_xy(0);
+ _velocity_setpoint(1) = vel_sp_xy(1);
/* Check for altitude lock.*/
_updateXYlock();
/* Smooth velocity in z.*/
- _smoothingZ.smoothVelFromSticks(_vel_sp(2), _deltatime);
+ _smoothingZ.smoothVelFromSticks(_velocity_setpoint(2), _deltatime);
/* Check for altitude lock*/
_updateAltitudeLock();
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp
index ea65e57725..a4efbcb870 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp
@@ -52,9 +52,7 @@ FlightTaskManualStabilized::FlightTaskManualStabilized(control::SuperBlock *pare
bool FlightTaskManualStabilized::activate()
{
- _resetToNAN();
- _yaw_rate_sp = 0.0f;
- _thr_sp = matrix::Vector3f(0.0f, 0.0f, -_throttle_hover.get());
+ _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -_throttle_hover.get());
return FlightTaskManual::activate();
}
@@ -62,8 +60,9 @@ void FlightTaskManualStabilized::_scaleSticks()
{
/* Scale sticks to yaw and thrust using
* linear scale for yaw and piecewise linear map for thrust. */
- _yaw_rate_sp = _sticks(3) * math::radians(_yaw_rate_scaling.get());
- _yaw_rate_sp = math::sign(_yaw_rate_sp) * math::min(fabsf(_yaw_rate_sp), math::radians(_yaw_rate_max.get()));
+ _yawspeed_setpoint = _sticks(3) * math::radians(_yaw_rate_scaling.get());
+ _yawspeed_setpoint = math::sign(_yawspeed_setpoint) * math::min(fabsf(_yawspeed_setpoint),
+ math::radians(_yaw_rate_max.get()));
_throttle = _throttleCurve();
}
@@ -74,11 +73,11 @@ void FlightTaskManualStabilized::_updateHeadingSetpoints()
* TODO: add yawspeed to get threshold.*/
const bool stick_yaw_zero = fabsf(_sticks(3)) <= _stick_dz.get();
- if (stick_yaw_zero && !PX4_ISFINITE(_yaw_sp)) {
- _yaw_sp = _yaw;
+ if (stick_yaw_zero && !PX4_ISFINITE(_yaw_setpoint)) {
+ _yaw_setpoint = _yaw;
} else if (!stick_yaw_zero) {
- _yaw_sp = NAN;
+ _yaw_setpoint = NAN;
}
}
@@ -111,12 +110,12 @@ void FlightTaskManualStabilized::_updateThrustSetpoints()
* upward by the Axis-Angle.
*/
Quatf q_sp = AxisAnglef(v(0), v(1), 0.0f);
- _thr_sp = q_sp.conjugate(Vector3f(0.0f, 0.0f, -1.0f)) * _throttle;
+ _thrust_setpoint = q_sp.conjugate(Vector3f(0.0f, 0.0f, -1.0f)) * _throttle;
}
void FlightTaskManualStabilized::_rotateIntoHeadingFrame(Vector2f &v)
{
- float yaw_rotate = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw;
+ float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw;
Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f));
v(0) = v_r(0);
v(1) = v_r(1);
@@ -147,11 +146,5 @@ bool FlightTaskManualStabilized::update()
_scaleSticks();
_updateSetpoints();
- _position_setpoint = _pos_sp;
- _velocity_setpoint = _vel_sp;
- _thrust_setpoint = _thr_sp;
- _yaw_setpoint = _yaw_sp;
- _yawspeed_setpoint = _yaw_rate_sp;
-
return true;
}
diff --git a/src/lib/FlightTasks/tasks/FlightTaskSport.hpp b/src/lib/FlightTasks/tasks/FlightTaskSport.hpp
index 590e6fd5b3..f8db313781 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskSport.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskSport.hpp
@@ -61,11 +61,11 @@ protected:
FlightTaskManualPosition::_updateSetpoints(); // get all setpoints from position task
/* Scale horizontal velocity setpoint by maximum allowed velocity. */
- if (PX4_ISFINITE(_vel_sp(0)) && Vector2f(&_vel_sp(0)).length() > 0.0f) {
- Vector2f vel_sp_xy = Vector2f(&_vel_sp(0));
+ if (PX4_ISFINITE(_velocity_setpoint(0)) && Vector2f(&_velocity_setpoint(0)).length() > 0.0f) {
+ Vector2f vel_sp_xy = Vector2f(&_velocity_setpoint(0));
vel_sp_xy = vel_sp_xy.normalized() * _vel_xy_max.get() / _vel_xy_manual_max.get() * vel_sp_xy.length();
- _vel_sp(0) = vel_sp_xy(0);
- _vel_sp(1) = vel_sp_xy(1);
+ _velocity_setpoint(0) = vel_sp_xy(0);
+ _velocity_setpoint(1) = vel_sp_xy(1);
}
}
--
GitLab