yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskOrbit: only reapproach circle if center changed

Browse Source 
The condition that the vehicle is more than 3m away from the circle line
was too sloppy. That often happens when the radius is changed by sticks.
A reapproach is only necessary when the center is moved and that's only
possible through the orbit command.
This commit is contained in:
Matthias Grob
2019-08-12 11:08:11 +02:00
parent cca7596bcd
commit 7774b150a9
1 changed files with 3 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp
View File

@@ -92,6 +92,9 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
} }
} }
// perpendicularly approach the orbit circle again when new parameters get commanded
_in_circle_approach = true;
return ret; return ret;
} }
@@ -179,12 +182,7 @@ bool FlightTaskOrbit::update()
setVelocity(v); setVelocity(v);
Vector2f center_to_position = Vector2f(_position) - _center; Vector2f center_to_position = Vector2f(_position) - _center;
const float distance_to_center = center_to_position.norm();
// perpendicularly approach the orbit circle if further away than 3 meters
if (!_in_circle_approach && !math::isInRange(distance_to_center, _r - 3.f, _r + 3.f)) {
_in_circle_approach = true;
}
if (_in_circle_approach) { if (_in_circle_approach) {
generate_circle_approach_setpoints(); generate_circle_approach_setpoints();