fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 20:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Matthias Grob
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent 88bf40e3cb
commit e51e5f3e01
1 changed files with 18 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
+18 -8
View File
@@ -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 */
};