FlightTaskOrbit: added constraints for parameters & defined linear velocity instead of angular

src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp

@@ -73,20 +73,30 @@ public:
 	{
 		FlightTask::update();
 		r += _sticks(1) * _deltatime;
-		vu += _sticks(0) * 0.1f * _deltatime;
+		r = math::constrain(r, 0.5f, 4.f);
+
+		v += _sticks(0) * _deltatime; /* angular velocity for orbiting in revolutions per second */
+		v = math::constrain(v, -4.f, 4.f);
 		altitude += _sticks(3) * _deltatime;
-		//printf("%f %f %f\n", (double)altitude, (double)r, (double)vu);
+		altitude = math::constrain(altitude, 2.f, 5.f);
+
+		printf("%f %f %f\n", (double)altitude, (double)r, (double)v);
+
 		//printf("%f %f %f\n", (double)_position(0), (double)_position(1), (double)_position(2));
-		printf("%f %f %f\n", (double)_velocity(0), (double)_velocity(1), (double)_velocity(2));
-		float v = 2 * M_PI_F * vu; /* velocity for orbiting in radians per second */
-		_set_position_setpoint(matrix::Vector3f(r * cosf(v * _time), r * sinf(v * _time), -altitude));
+		//printf("%f %f %f\n", (double)_velocity(0), (double)_velocity(1), (double)_velocity(2));
+		if (fabsf(r) < 0.01f) {
+			r = 0.01;
+		}
+
+		float w = v / r; /* angular velocity for orbiting in radians per second */
+		_set_position_setpoint(matrix::Vector3f(r * cosf(w * _time), r * sinf(w * _time), -altitude));
 		return 0;
 	};
 
 private:
 
-	float r = 2.f; /* radius with which to orbit the target */
-	float vu =  0.1f; /* velocity for orbiting in revolutions per second */
-	float altitude = 2; /* altitude in meters */
+	float r = 1.f; /* radius with which to orbit the target */
+	float v =  0.1f; /* linear velocity for orbiting in m/s */
+	float altitude = 2.f; /* altitude in meters */
 
 };