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

FlightTaskOrbit: Direct orbit approach with slowdown at intersection point

Browse Source
This commit is contained in:
Thomas Debrunner
2021-11-03 17:37:52 +01:00
committed by Matthias Grob
parent 0d0b87e193
commit d450afead6
2 changed files with 3 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
+1 -30
View File
@@ -280,26 +280,6 @@ bool FlightTaskOrbit::_is_position_on_circle() const
}
bool circle_tangents_for_position(const Vector2f &center, float radius, const Vector2f &position, Vector2f &out1,
Vector2f &out2)
{
const Vector2f d = position - center;
const Vector2f dr = {-d(1), d(0)};
float d_norm = d.length();
if (d_norm >= radius) {
float rho = radius / d_norm;
float ad = rho * rho;
float bd = rho * sqrtf(1.f - ad);
out1 = center + ad * d + bd * dr;
out2 = center + ad * d - bd * dr;
return true;
}
return false;
}
void FlightTaskOrbit::_generate_circle_approach_setpoints()
{
const Vector2f center2d = Vector2f(_center);
@@ -307,16 +287,7 @@ void FlightTaskOrbit::_generate_circle_approach_setpoints()
Vector2f closest_point_on_circle = Vector2f(_position) + position_to_center_xy.unit_or_zero() *
(position_to_center_xy.norm() - _orbit_radius);
float angle = math::radians(8.f);
float s_a = _orbit_velocity >= 0 ? sinf(angle) : -sinf(angle);
float c_a = cosf(angle);
Vector2f origin_closest = (closest_point_on_circle - center2d);
Vector2f target_circle_point_xy = {
center2d(0) + c_a * origin_closest(0) - s_a * origin_closest(1),
center2d(1) + s_a * origin_closest(0) + c_a * origin_closest(1)
};
const Vector3f target_circle_point{target_circle_point_xy(0), target_circle_point_xy(1), _center(2)};
const Vector3f target_circle_point{closest_point_on_circle(0), closest_point_on_circle(1), _center(2)};
PositionSmoothing::PositionSmoothingSetpoints out_setpoints;
_position_smoothing.generateSetpoints(_position, {
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
+2 -2
View File
@@ -114,8 +114,8 @@ private:
/** generates yaw setpoints to control the vehicle's heading */
void _generate_circle_yaw_setpoints();
float _orbit_velocity;
float _orbit_radius;
float _orbit_velocity{};
float _orbit_radius{};
matrix::Vector3f _center; /**< local frame coordinates of the center point */
bool _in_circle_approach = false;