diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp index de34f7b6be..0c0426b175 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp @@ -50,15 +50,44 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) const float &r = command.param3; // commanded radius const float &v = command.param4; // commanded velocity - if (math::isInRange(r, 5.f, 50.f) && fabs(v) < 10.f) { - _r = r; - _v = v; + if (setRadius(r) && setVelocity(v)) { return FlightTaskManual::applyCommandParameters(command); } return false; } +bool FlightTaskOrbit::setRadius(const float r) +{ + if (math::isInRange(r, radius_min, radius_max)) { + // radius is more important than velocity for safety + if (!checkAcceleration(r, _v, acceleration_max)) { + _v = sqrtf(acceleration_max * r); + } + + _r = r; + return true; + } + + return false; +} + +bool FlightTaskOrbit::setVelocity(const float v) +{ + if (fabs(v) < velocity_max && + checkAcceleration(_r, v, acceleration_max)) { + _v = v; + return true; + } + + return false; +} + +bool FlightTaskOrbit::checkAcceleration(float r, float v, float a) +{ + return v * v < a * r; +} + bool FlightTaskOrbit::activate() { bool ret = FlightTaskManual::activate(); @@ -81,12 +110,14 @@ bool FlightTaskOrbit::activate() bool FlightTaskOrbit::update() { - _r += _sticks_expo(0) * _deltatime; - _r = math::constrain(_r, 1.f, 20.f); - _v -= _sticks_expo(1) * _deltatime; - _v = math::constrain(_v, -7.f, 7.f); + // stick input adjusts parameters + const float r = _r + _sticks_expo(0) * _deltatime; + const float v = _v - _sticks_expo(1) * _deltatime; _z += _sticks_expo(2) * _deltatime; + setRadius(r); + setVelocity(v); + _position_setpoint = Vector3f(NAN, NAN, _z); // xy velocity to go around in a circle diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp index 1a71d6d154..e83fc70eea 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp @@ -47,19 +47,47 @@ class FlightTaskOrbit : public FlightTaskManual { public: FlightTaskOrbit(); - virtual ~FlightTaskOrbit() = default; bool applyCommandParameters(const vehicle_command_s &command) override; - bool activate() override; - bool update() override; + /** + * Check the feasibility of orbit parameters with respect to + * centripetal acceleration a = v^2 / r + * @param r desired radius + * @param v desired velocity + * @param a maximal allowed acceleration + * @return true on success, false if value not accepted + */ + bool checkAcceleration(float r, float v, float a); + +protected: + + /** + * Change the radius of the circle. + * @param r desired new radius + * @return true on success, false if value not accepted + */ + bool setRadius(const float r); + + /** + * Change the velocity of the vehicle on the circle. + * @param v desired new velocity + * @return true on success, false if value not accepted + */ + bool setVelocity(const float v); + private: float _r = 0.f; /**< radius with which to orbit the target */ float _v = 0.f; /**< linear velocity for orbiting in m/s */ float _z = 0.f; /**< local z coordinate in meters */ matrix::Vector2f _center; + // TODO: create/use parameters for limits + const float radius_min = 1.f; + const float radius_max = 20.f; + const float velocity_max = 10.f; + const float acceleration_max = 2.f; };