diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp
index 4e2fb498f21ca8b36481664e4aebac321a1bc8a4..f482aa929b607907bd1b29667f131a3d599f6d6d 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 b5abb21e3e845d81ead3466119efa352abacd57a..fe0a650ddde3385b809e7562bfe77f4b0a1c32e7 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 b46ab06dc85b27579651343a35895c25ef7981f6..7d40ebbe5114f8b4af57088fca3de3f9788a1844 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 f018b1ad240009d4b55e4cb0fc7992632567e642..bf2da20fb9d3eb0b8dc3022a26cdffb70f1accfb 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 4b3882bae8002a482a3019d714492b3eea8510bb..ea65e577259f01336374eb80186bfe0e6d674ce9 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 86973dd29d8816cf34440b697e98293e65bf497c..3af14f8147b2a205c031e1b27b2b1fe3c94be284 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;
}