FlightTaskOrbit: add acceleration limit & unify checks

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

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;
 };