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

FlightTasks: added possibility to set velocity setpoint, switched Orbit to feedback velocity algorithm

Browse Source
This commit is contained in:
Matthias Grob
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent e51e5f3e01
commit 73f633dfd2
1 changed files with 16 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
+16 -11
View File
@@ -72,24 +72,29 @@ public:
virtual int update() virtual int update()
{ {
FlightTask::update(); FlightTask::update();
r += _sticks(1) * _deltatime; r += _sticks(1) * _deltatime;
r = math::constrain(r, 0.5f, 4.f); r = math::constrain(r, 0.5f, 4.f);
v -= _sticks(0) * _deltatime;
v += _sticks(0) * _deltatime; /* angular velocity for orbiting in revolutions per second */ v = math::constrain(v, -7.f, 7.f);
v = math::constrain(v, -4.f, 4.f);
altitude += _sticks(3) * _deltatime; altitude += _sticks(3) * _deltatime;
altitude = math::constrain(altitude, 2.f, 5.f); altitude = math::constrain(altitude, 2.f, 5.f);
printf("%f %f %f\n", (double)altitude, (double)r, (double)v); matrix::Vector2<float> target_to_position = matrix::Vector2f(_position.data()) - matrix::Vector2f();
// TODO: add local frame target position here
//printf("%f %f %f\n", (double)_position(0), (double)_position(1), (double)_position(2)); /* xy velocity to go around in a circle */
//printf("%f %f %f\n", (double)_velocity(0), (double)_velocity(1), (double)_velocity(2)); matrix::Vector2<float> velocity_xy = matrix::Vector2f(target_to_position(1), -target_to_position(0));
if (fabsf(r) < 0.01f) { velocity_xy.normalize();
r = 0.01; velocity_xy *= v;
}
float w = v / r; /* angular velocity for orbiting in radians per second */ /* xy velocity adjustment to stay on the radius distance */
_set_position_setpoint(matrix::Vector3f(r * cosf(w * _time), r * sinf(w * _time), -altitude)); velocity_xy += (r - target_to_position.norm()) * target_to_position.normalized();
//printf("%f %f %f\n", (double)altitude, (double)r, (double)v);
_set_position_setpoint(matrix::Vector3f(NAN, NAN, -altitude));
_set_velocity_setpoint(matrix::Vector3f(velocity_xy(0), velocity_xy(1), 0.f));
return 0; return 0;
}; };