From 309237c4a2824e43875deb39ef731387a3eaeae8 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Wed, 28 Feb 2018 09:30:20 +0100
Subject: [PATCH] FlightTasks: replace setpoint setters with members
I realized that instead of using the setpoint setters inline
in real world tasks everyone started to have its own setpoint
member variable and only call the setter in the end for all the
privatly generate setpoints. This makes the setter useless and
therefore I switched to member setpoints in the architecture.
They bring more felxibility which is obviously needed but also
less structure which is the price to pay.
---
src/lib/FlightTasks/FlightTasks.cpp | 2 +-
src/lib/FlightTasks/FlightTasks.hpp | 7 +---
src/lib/FlightTasks/tasks/FlightTask.cpp | 25 ++++++++++-
src/lib/FlightTasks/tasks/FlightTask.hpp | 41 +++++++------------
.../tasks/FlightTaskManualStabilized.cpp | 10 ++---
src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp | 10 ++---
6 files changed, 51 insertions(+), 44 deletions(-)
diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp
index 4e2fb498f2..f482aa929b 100644
--- a/src/lib/FlightTasks/FlightTasks.cpp
+++ b/src/lib/FlightTasks/FlightTasks.cpp
@@ -15,7 +15,7 @@ bool FlightTasks::update()
return false;
}
-const vehicle_local_position_setpoint_s &FlightTasks::getPositionSetpoint()
+const vehicle_local_position_setpoint_s FlightTasks::getPositionSetpoint()
{
if (isAnyTaskActive()) {
return _current_task->getPositionSetpoint();
diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp
index b5abb21e3e..fe0a650ddd 100644
--- a/src/lib/FlightTasks/FlightTasks.hpp
+++ b/src/lib/FlightTasks/FlightTasks.hpp
@@ -91,12 +91,7 @@ public:
* Get the output data from the current task
* @return output setpoint, to be executed by position control
*/
- const vehicle_local_position_setpoint_s &getPositionSetpoint();
-
- /**
- * Convenient operator to get the output data from the current task
- */
- inline const vehicle_local_position_setpoint_s &operator()() { return getPositionSetpoint(); }
+ const vehicle_local_position_setpoint_s getPositionSetpoint();
/**
* Switch to the next task in the available list (for testing)
diff --git a/src/lib/FlightTasks/tasks/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask.cpp
index b46ab06dc8..7d40ebbe51 100644
--- a/src/lib/FlightTasks/tasks/FlightTask.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTask.cpp
@@ -16,13 +16,13 @@ bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
bool FlightTask::activate()
{
+ _resetSetpoints();
_time_stamp_activate = hrt_absolute_time();
return true;
}
bool FlightTask::updateInitialize()
{
- _resetSetpoint();
_time_stamp_current = hrt_absolute_time();
_time = (_time_stamp_current - _time_stamp_activate) / 1e6f;
_deltatime = math::min((_time_stamp_current - _time_stamp_last), _timeout) / 1e6f;
@@ -30,6 +30,29 @@ bool FlightTask::updateInitialize()
return _evaluateVehiclePosition();
}
+const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
+{
+ /* fill position setpoint message */
+ vehicle_local_position_setpoint_s vehicle_local_position_setpoint;
+ vehicle_local_position_setpoint.timestamp = hrt_absolute_time();
+ _position_setpoint.copyToRaw(&vehicle_local_position_setpoint.x);
+ _velocity_setpoint.copyToRaw(&vehicle_local_position_setpoint.vx);
+ _acceleration_setpoint.copyToRaw(&vehicle_local_position_setpoint.acc_x);
+ _thrust_setpoint.copyTo(vehicle_local_position_setpoint.thrust);
+ vehicle_local_position_setpoint.yaw = _yaw_setpoint;
+ vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint;
+ return vehicle_local_position_setpoint;
+}
+
+void FlightTask::_resetSetpoints()
+{
+ _position_setpoint *= NAN;
+ _velocity_setpoint *= NAN;
+ _acceleration_setpoint *= NAN;
+ _thrust_setpoint *= NAN;
+ _yaw_setpoint = _yawspeed_setpoint = NAN;
+}
+
bool FlightTask::_evaluateVehiclePosition()
{
if ((_time_stamp_current - _sub_vehicle_local_position->get().timestamp) < _timeout) {
diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp
index f018b1ad24..bf2da20fb9 100644
--- a/src/lib/FlightTasks/tasks/FlightTask.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTask.hpp
@@ -56,7 +56,7 @@ class FlightTask : public control::Block
public:
FlightTask(control::SuperBlock *parent, const char *name) :
Block(parent, name)
- { _resetSetpoint(); }
+ { _resetSetpoints(); }
virtual ~FlightTask() = default;
@@ -94,10 +94,7 @@ public:
/**
* Get the output data
*/
- const vehicle_local_position_setpoint_s &getPositionSetpoint()
- {
- return _vehicle_local_position_setpoint;
- }
+ const vehicle_local_position_setpoint_s getPositionSetpoint();
static const vehicle_local_position_setpoint_s empty_setpoint;
@@ -110,35 +107,27 @@ protected:
hrt_abstime _time_stamp_current = 0; /**< time stamp at the beginning of the current task update */
hrt_abstime _time_stamp_last = 0; /**< time stamp when task was last updated */
- /* Current vehicle position */
+ /* Current vehicle state */
matrix::Vector3f _position; /**< current vehicle position */
matrix::Vector3f _velocity; /**< current vehicle velocity */
float _yaw = 0.f;
- /* Put the position vector produced by the task into the setpoint message */
- void _setPositionSetpoint(const matrix::Vector3f &position_setpoint) { position_setpoint.copyToRaw(&_vehicle_local_position_setpoint.x); }
+ /* Setpoints the position controller needs to execute
+ * NAN values mean the state does not get controlled */
+ matrix::Vector3f _position_setpoint;
+ matrix::Vector3f _velocity_setpoint;
+ matrix::Vector3f _acceleration_setpoint;
+ matrix::Vector3f _thrust_setpoint;
+ float _yaw_setpoint;
+ float _yawspeed_setpoint;
- /* Put the velocity vector produced by the task into the setpoint message */
- void _setVelocitySetpoint(const matrix::Vector3f &velocity_setpoint) { velocity_setpoint.copyToRaw(&_vehicle_local_position_setpoint.vx); }
-
- /* Put the acceleration vector produced by the task into the setpoint message */
- void _setAccelerationSetpoint(const matrix::Vector3f &acceleration_setpoint) { acceleration_setpoint.copyToRaw(&_vehicle_local_position_setpoint.acc_x); }
-
- /* Put the yaw angle produced by the task into the setpoint message */
- void _setYawSetpoint(const float yaw) { _vehicle_local_position_setpoint.yaw = yaw; }
-
- /* Put the yaw anglular rate produced by the task into the setpoint message */
- void _setYawspeedSetpoint(const float &yawspeed) { _vehicle_local_position_setpoint.yawspeed = yawspeed; }
-
- /* Put the thrust setpoint produced by the task into the setpoint message */
- void _setThrustSetpoint(const matrix::Vector3f &thrust_setpoint) { thrust_setpoint.copyTo(_vehicle_local_position_setpoint.thrust); }
+ /**
+ * Get the output data
+ */
+ void _resetSetpoints();
private:
uORB::Subscription<vehicle_local_position_s> *_sub_vehicle_local_position{nullptr};
- vehicle_local_position_setpoint_s _vehicle_local_position_setpoint; /**< Output position setpoint that every task has */
-
- void _resetSetpoint() { _vehicle_local_position_setpoint = empty_setpoint; }
-
bool _evaluateVehiclePosition();
};
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp
index 4b3882bae8..ea65e57725 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp
@@ -147,11 +147,11 @@ bool FlightTaskManualStabilized::update()
_scaleSticks();
_updateSetpoints();
- _setPositionSetpoint(_pos_sp);
- _setVelocitySetpoint(_vel_sp);
- _setThrustSetpoint(_thr_sp);
- _setYawSetpoint(_yaw_sp);
- _setYawspeedSetpoint(_yaw_rate_sp);
+ _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/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp
index 86973dd29d..3af14f8147 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp
@@ -79,9 +79,10 @@ bool FlightTaskOrbit::update()
_v = math::constrain(_v, -7.f, 7.f);
_z += _sticks_expo(2) * _deltatime;
- Vector2f center_to_position = Vector2f(_position.data()) - _center;
+ _position_setpoint = Vector3f(NAN, NAN, _z);
/* xy velocity to go around in a circle */
+ Vector2f center_to_position = Vector2f(_position.data()) - _center;
Vector2f velocity_xy = Vector2f(center_to_position(1), -center_to_position(0));
velocity_xy = velocity_xy.unit_or_zero();
velocity_xy *= _v;
@@ -89,10 +90,9 @@ bool FlightTaskOrbit::update()
/* xy velocity adjustment to stay on the radius distance */
velocity_xy += (_r - center_to_position.norm()) * center_to_position.unit_or_zero();
- float yaw = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F;
+ _velocity_setpoint = Vector3f(velocity_xy(0), velocity_xy(1), 0.f);
- _setPositionSetpoint(Vector3f(NAN, NAN, _z));
- _setVelocitySetpoint(Vector3f(velocity_xy(0), velocity_xy(1), 0.f));
- _setYawSetpoint(yaw);
+ /* make vehicle front always point towards the center */
+ _yawspeed_setpoint = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F;
return true;
}
--
GitLab